The VFO-Driven Motion Planning and Feedback Control in Polygonal Worlds for a Unicycle with Bounded Curvature of Motion

Integrated motion planning and control for the purposes of maneuvering mobile robots under state- and input constraints is a problem of vital practical importance in applications of mobile robots such as autonomous transportation. Those constraints arise naturally in practice due to specifics of robot mechanical construction and the presence of obstacles in motion environment. In contrast to approaches focusing on feedback control design under the assumption of given reference motion or motion planning with neglection of subsequent feedback motion execution, we adopt a controller-driven motion planning paradigm, which has recently gained attention of many researchers. It postulates design of motion planning algorithms dedicated to specific feedback control policies, which compute a sequence of feedback control subtasks instead of classically planned open-loop controls or parametric paths. In this spirit, we propose a motion planning algorithm driven by the VFO (Vector Field Orientation) control law for the waypoint-following task. Presented analysis of the VFO control law reveals its beneficial properties, which are subsequently utilized to solve a generally nonlinear and non-convex optimal motion planning problem by formulating it as a mixed-integer linear program (MILP). The solution proposed in this paper yields a waypoint sequence, which is designed for execution by application of the VFO control law to drive a robot to a prescribed final configuration under an input constraint imposed by bounded curvature of robot motion and state constraints resulting from a convex decomposition of task space. Satisfaction of these constraints is guaranteed analytically and exactly, i.e., without utilization of numerical approximations. Moreover, for a given discrete set of possible waypoint orientations, the proposed algorithm computes plans optimal w.r.t. given cost functional, which can be any convex linear combination of quantities such as robot path length, curvature of robot motion, distance to imposed state constraints, etc. Furthermore, the planning algorithm exploits the possibility of both forward or backward movement of the robot to allow maneuvering in demanding environments. Generated waypoint sequences are a compact representation of a motion plan, which can be immediately executed with the VFO controller without any additional post-processing. Validity of the proposed approach has been confirmed by simulation studies and experimental motion execution with a laboratory-scale mobile robot.

a motion planning algorithm driven by the VFO (Vector Field Orientation) control law for the waypointfollowing task.Presented analysis of the VFO control law reveals its beneficial properties, which are subsequently utilized to solve a generally nonlinear and non-convex optimal motion planning problem by formulating it as a mixed-integer linear program (MILP).The solution proposed in this paper yields a waypoint sequence, which is designed for execution by application of the VFO control law to drive a robot to a prescribed final configuration under an input constraint imposed by bounded curvature of robot motion and state constraints resulting from a convex decomposition of task space.Satisfaction of these constraints is guaranteed analytically and exactly, i.e., without utilization of numerical approximations.Moreover, for a given discrete set of possible waypoint orientations, the proposed algorithm computes plans optimal w.r.t.given cost functional, which can be any convex linear combination of quantities such as robot path length, curvature of robot motion, distance to imposed state constraints, etc.Furthermore, the planning algorithm exploits the possibility of both forward or backward movement of the robot to allow maneuvering in demanding environments.Generated waypoint sequences are a compact representation of a motion plan, which can be immediately executed with the VFO controller without any additional postprocessing.Validity of the proposed approach has

Introduction
The ability to perform autonomous maneuvers in cluttered environments is desired in many practical applications of wheeled mobile robots such as searchand-rescue missions, warehouse management automation, service robotics, parking assitance systems, and lately autonomous road vehicles.In this work we propose a controller-driven motion planning algorithm and feedback control strategy for a generic robot-body model in the form of a kinematic unicycle.It is simple and captures nonholonomic constraints present in various autonomous vehicles of interest, such as N-trailer trucks, cars and differentially-driven robots.Thus, it can serve as a good starting point for extensions of our method to more complex kinematics.Mechanical construction of vehicles often imposes additional limitations on robot motion resulting from, e.g., bounded steering angle in case of car-like vehicles and bounded articulation angles in case of N-trailer vehicles.Those limitations are reflected in the kinematic robot model by an input constraint imposing bounded curvature of motion.Considered robot model is complemented by state constraints, which can directly represent a model of the cluttered environment or designated safe motion areas.
Traditionally, the motion planning problem is solved by implementing a multi-stage sequential planning procedure as mentioned, e.g, in [29] and implemented, e.g., in [27].Such planners often generate a purely geometric, and in some cases initially infeasible, collision-free path in configuration space, which is subsequently parametrized with time (see [50] for an energy-optimal approach) and optionally smoothed (as demonstrated in [9]) to form a trajectory.During the motion execution stage, the computed path or trajectory can be tracked using one of the available feedback controllers.Although such an approach yields acceptable results, there are various limitations resulting from the fact that behavior of the closed-loop motion control system is not explicitly considered during motion planning stage.The practically important effects of non-nominal motion conditions (e.g., initial robot configuration placed away from the planned one), exogenous disturbances, and uncertain measurements may render a feasible motion plan unfit for safe execution with a given feedback control law in practical conditions.
We adopt a controller-driven motion planning design paradigm envisioned in [29] under the name of algorithmic control theory and applied in [5,44] as policy based motion planning.The controller-driven methodology postulates design of specialized motion planners dedicated to specific feedback motion controllers.Such a specialization makes it possible to generate a motion plan tailored specifically to the needs of the assumed feedback control law.At the same time, the availability of information about closed-loop system behavior enables the planner to exploit specific properties of the assumed controller leading to a potentially simpler and more efficient planning process.The same information can be also utilized to avoid motion conditions, which are adverse for the utilized feedback control law by appropriate motion planning.
In this paper, we extend the results presented in [12] and propose a controller-driven motion planner for a kinematic unicycle with bounded curvature of motion and state constraints.Constraints imposed on the robot state are described by a sequence of traversable convex polygons (convex decomposition), which is a convenient representation for cluttered motion environments.Presented motion planning algorithm is dedicated to the VFO (Vector Field Orientation) controller in the version for the waypoint-following task proposed in [34] as an extension of the methodology presented in [8].The design of our planner relies upon specific beneficial properties of the VFO control law, some of which were found recently and were presented in the preliminary work [12].Thanks to those properties the generally nonlinear and non-convex planning problem was casted to a mixed-integer linear program (MILP) in our specific case.As argued later in Section 5, the complexity of formulated problem depends on topology of the environment as opposed to the methods employing direct time-discretization of robot control signals.What is more, satisfaction of state and curvature of motion constraints is guaranteed analytically, while planning of waypoint positions is conducted in a continuous domain.Another closely related benefit of the controller-driven design methodology is the fact that our algorithm generates motion plans represented compactly as short sequences of waypoint configurations, which are executed directly (i.e.without any postprocessing) by the VFO control law.Although motion plans contain only a small number of waypoints, robot path during plan execution is perceived as natural thanks to application of the VFO controller.
In the sequel, we systematically describe the design and verification of the proposed motion planning algorithm.In Section 2 we shortly review motion planning and control algorithms for kinematic models similar to the assumed constrained unicycle model.In Section 3 we formally state the problem, introduce notational conventions and sketch a general strategy of our solution.Section 4 contains introductory information regarding the VFO control law necessary for further considerations and the description of its specific properties useful in motion planning.Section 5 provides a more detailed description of the motion planning algorithm.For simplicity of exposition the method is presented first without consideration of the state constraints, and later extended to the convex polygon sequences.In Section 6 we touch upon the issues of practical motion execution.Section 7 contains results of simulations and experimental verification of the proposed solution for selected scenarios.Results of motion execution in non-nominal motion conditions arising from practical issues demonstrate viability of our approach.The work is summarized with final remarks gathered in Section 8.

Related Work
Let us briefly review selected motion planning algorithms for mobile robots with bounded curvature of motion in the presence of state constraints representing cluttered environment.Emphasis is put on controller-driven methods.A wider review of motion planning methods for mobile robots can be found in [28] and [26].
In [32] a particular polynomial spline analyzed in [41] was utilized in conjunction with a multicriterial optimization algorithm to plan autonomous parking maneuvers.This solution, although attractive due to its ability to generate G 3 -continuous paths, assumes a fixed number of splines and predefined motion sense (forward or backward robot motion) dependent on specific maneuver type and motion environment topology.According to the authors of [32], those assumptions can be lifted at the expense of utilizing stochastic global optimization algorithms, which are characterized by high computational cost and potential convergence problems.Even under mentioned assumptions, the authors of [32] design a nonlinear optimization problem, for which in general only locally optimal paths are attained.Moreover, to satisfy state and curvature constraints, discretization in the domains of robot configuration and path parameters must be performed.Those issues remain relevant for various other methods based on nonlinear optimization or direct nonlinear optimal control such as [16].Nevertheless, a promising local planning and control system utilizing a similar methods was proposed in [51].
One can also find several approaches relying on formulation of a MILP (mixed-integer linear programming) problem for model-predictive control or planning, utilizing discrete time problem formulations with explicit discretization of controls such as [3,19,46,53].Another interesting approach shown in [49] depends upon consideration of obstacle avoidance problem using optimal control framework.
Another approach shown in, e.g., [31,37,42,48,55] relies on utilization of graph search algorithms to find a path in a lazily evaluated graph representing discrete points in state space connected by reference motions taken from a finite set of precomputed motion primitives.In the same spirit, an interesting combination of purely geometric space exploration followed by motion primitive based planning similar to [45] was proposed in [4].Such an approach is very flexible, since it utilizes a well known versatile grid-based environment representation and makes it conceptually easy to consider nearly arbitrary nonlinear system dynamics.On the other hand, such lattice based planners may have high memory requirements and can be applied only for discretized dynamical systems.
Moreover, care must be taken when designing a finite set of motion primitives utilized in various sampling-based or lattice-based planners.Many motion primitive sets constructed by sampling of various control signals may produce path segments hard to describe or approximate analytically, posing issues for feedback control during motion execution.This problem can be alleviated by sampling in the space induced by a family of control policies.Such a concept is in fact very similiar to the work presented in [5].Worth noting that the assumption of a discrete set of motion primitives excludes asymptotic convergence of a computed motion plan to a solution, which is globally optimal w.r.t.assumed cost functional as shown, e.g, in [38].
Recently, we observe fast development of sampling-based motion planners building upon the concept of RRT (Rapidly Random exploring Trees) [11,15,17,20,23,30] and other closely related probabilistic approaches such as [13].Their controller-driven variants are usually obtained by integration of a specific extend procedure into the planner.Utilized extend procedures range from general optimal-control approches such as [40], through spline path segments (e.g., [18,54]), to more controller-driven approaches such as simulation of mobile robot with closed-loop control system proposed in [38] and control-Lyapunov function approach from [39].Sampling-based algorithms are attractive due to their generality and ability to cope with virtually any robot and motion environment model.On the other hand, in general, they guarantee only probabilistic completeness as opposed to resolution completeness and can suffer from slow convergence to optimal plans for nonholonomic systems.What is more, in some cases their computational efficiency can deteriorate in environments with very narrow passages due to their otherwise beneficial Voronoi bias property.However, the intensity of this effect seems to be rarely high enough to severely impact planning on modern hardware.It is worth noting, that their efficient implementation can be a complex task due to auxiliary algorithmic problems such as, e.g., nearest-neighbor search.
Reviewed planning algorithms are often complemented by methods used for postprocessing suboptimal plans using local optimization algorithms such as [24,56].In the same manner, one can utilize path generation methods [1,10] providing continuous-curvature approximations of Dubins [6] and Reeds/Shepp [43] paths, which are time-optimal and highly applicable to cluttered environments (see [2]).
The presence of various issues in the above reviewed motion planning algorithms justifies further investigation of the problem and development of alternative solutions.One such approach is the method based on global optimization, which is proposed in this work .Namely, in contrast to the method proposed in [32], we formulate a MILP optimization problem to find a sequence of waypoints, and, as a consequence, ensure convergence to global optimum w.r.t.assumed criteria and constraints for a given waypoint orientation discretization resolution.The number of motion segments, which is related to the number of splines in [32], does not need to be fixed but it can be fixed if the motion task at hand specifically requires so.Note that the proposed method considers optimization in the whole motion environment at once as opposed to [49].While such a global approach can potentially lead to better solutions, it is also characterized by larger computational cost.The local method from [49] avoids the high computational cost and potential infeasibility thanks to consideration of one obstacle at a time.This allows for fast online replanning, which is usually not attained by the global methods.It is also worth noting that our approach requires only discretization in the space of waypoint orientations.Waypoint positions are planned in a continuous domain, while state constraints and curvature constraints are checked exactly, i.e., without any approximations and using analytically derived conditions.This differentiates the proposed method from the ones available in the literature and is congruent with the ongoing trend of seeking more efficient constraint checking procedures (visible in works such as [22]), and with efforts to guarantee constraint satisfaction in a continuous domain, see e.g., [25].The benefits and necessity of seeking feedback control policies as opposed to open-loop controls or parametric paths has been also argued extensively in [5,29,38,39,44].
In our approach we utilize MILP optimization, which is also used in other MILP-based methods such as [3,19,46,53].However, there are significant differences between those methods and our approach.Namely, the method proposed in [53] is applicable only to linear dynamical systems (specifically, authors assumed point-mass dynamics), while in the present paper we consider unicycle kinematics with bounded curvature of motion, which is a strongly nonlinear system with a nonholonomic constraint and input constraints imposed.Since other MILP-based methods generate optimization constraints directly based on system dynamics (as opposed to our method leveraging the VFO control law), they are inherently limited to only linear systems, since other differential constraints cannot be expressed as linear constraints in a MILP.Thus, other MILP methods cannot be directly applied to a kinematic unicycle.Note that due to the presence of a nonholonomic constraint, unicycle kinematics cannot be linearized without serious consequences.Namely, if one applies a local Taylor linearization at a constant operating point (such as a waypoint configuration) to the unicycle kinematics, the resultant linear system is not fully controllable as shown in [36].The linearization of unicycle kinematics with a time-varying operating point is also not possible in the case of our problem, since the location of this point is not known at planning stage.On the other hand, application of the input-output feedback linearization loop to unicycle kinematics results in a loss of direct control over a robot orientation and ability to only control a position of an auxiliary point displaced w.r.t. the guidance point of a robot platform, which is also not acceptable for our problem.Even if one neglects the above arguments, makes an assumption of a linear point-mass dynamics model from [53], and attempts to incorporate the curvature constraint by constraining curvature of a path drawn by a position output of dynamics given in [53], one arrives at a nonlinear constraint resulting from curvature constraint involving velocity and acceleration components, which cannot be represented as a linear function of the variables introduced in [53].Similar arguments can be formulated for other MILP-based methods.
Moreover, MILP-based methods considered in the literature guarantee constraint satisfaction only for a discrete set of time instants, while our approach gives such guarantees in continuous time and configuration domain (even though waypoint orientations are chosen from a discrete set).It is also worth emphasizing that the algorithms from [3,19,46,53] plan piecewise-constant open-loop control signals, while our method generates a sequence of feedback control policies.The policies are ready to execute and they generate a continuously varying feedback control signal (everywhere except a finite sequence of switching points between the waypoints).Precise maneuvers can be planned with a relatively low number of waypoints due to consideration of a continuous planning domain as opposed to a finite set of motion primitives as proposed in methods utilizing grid search.On the other hand, design of a feedback controller for the plan provided by a classical MILP-based planner generating open-loop controls is a nontrivial problem.The same observations can be made for other similar approaches such as [3] and [46].Note that the controller-driven sampling-based methods such as [38,39,44] guarantee satisfaction of state and curvature constraints using discretization.
It is also important to note, that the proposed controller-driven planning method has certain limitations.Namely, it is characterized by a relatively large computational cost due to the fact, that it involves solving an optimization problem formulated as MILP, which is known to be NP-hard, as opposed to several other formulations.Furthermore, as stated in Section 3, our approach requires exact geometric knowledge of a robot motion environment.The presence of uncertainties is not explicitly considered.While uncertainties can be possibly handled by conservatively formulating state constraints and by online replanning, such an approach might be time consuming due to the computational cost of MILP optimization.However, in contrast to local planning methods, our planning method is designed as a global one.

System Model and Notation
We model a mobile robot as a kinematic unicycle with bounded curvature of motion where Q denotes robot configuration space with an admissible subset Q f ree defined by state constraints expressed as an admissible positions set P f ree (defined in assumption A4), while q is a configuration vector and u = [u 1 u 2 ] ∈ U ⊂ R 2 denotes control input which consists of robot body angular velocity u 1 and longitudinal velocity u 2 of guidance point q shown in Fig. 1.Curvature bound κ b is a prescribed constant resulting from robot motion task specification.It determines input constraint (3) defining the set Thus, as argued in the Introduction, it can be utilized as a basic building block for modeling more complex kinematics and motion task specifications.For example, constraint (3) can be defined by steeringangle and articulation-angle bounds resulting from mechanical construction of car-like and tractor-trailer robots, respectively, or prescribed motion smoothness requirements resulting from, e.g., fragile payload carried by the robot.
In this paper we assume that the robot is driven through a sequence W of N + 1 waypoint configurations, which is also called a plan.It is defined as follows: where q di is the i-th waypoint.During the subsequent analysis we often express variables in coordinate frame fixed at the (i + 1)-st waypoint.Symbols expressed in the local coordinate frame {i + 1} are denoted with upper index " i+1 ".Variables denoted without an upper index are expressed in the global coordinate frame.For example, q i+1 di = denotes the i-th waypoint expressed in coordinate frame fixed at the (i + 1)-st waypoint as shown in Fig. 2. Plan W splits robot motion execution process into N segments with index i denoting a motion segment number.We say that the robot is in the i-th motion segment, when it is driven towards (i + 1)-st waypoint.The current ith segment is switched to the (i+1)-st one at time t i+1 , as defined in assumption A2.We denote by τ i t − t i the relative motion-segment time.

Problem Statement and Assumptions
Let us investigate the controller-driven motion planning task dedicated to the VFO control law and defined as follows.
Problem 1 Given an admissible subset of configuration space Q f ree ⊆ Q, an initial configuration q(t = 0) ∈ Q f ree , and goal configuration q dN ∈ Q f ree , the objective is to plan a waypoint configuration sequence W (cf. ( 4)) such that its sequential execution with the VFO control law applied to system (1) with constraint (3) guarantees that and with T ∈ (0, ∞) being a control time-horizon.Problem 1 shall be solved under the following assumptions: A0. q d0 := q(t = 0) A1.Robot body is represented by a point in Q f ree and Q f ree is appropriately shrinked to account for actual robot body size.A2.The robot switches motion execution from the i-th motion segment to the (i + 1)-st one at time t i+1 , when qi+1 (t i+1 ) ≤ , where || • || denotes the Euclidean norm, and ≥ 0 is a design parameter.A3.Every robot orientation θ is admissible.A4.The set of admissible robot positions P f ree is known a priori and given by sequence of M + 1 convex polygons where ∂P f ree denotes the boundary of set P f ree .A5. Subsequent polygons share a transition edge E j +1 , that is, A6.No polygon in P f ree contains an edge co-linear with vertical line x = c, c = const in the global frame.A7.Polygons shall be visited sequentially, i.e., ∀j ∈ {0, 1, . . ., M − 1} ∀t ∈ [t j , t j +1 ] q(t) ∈ P j , and ∀j ∈ {0, 1, . . ., M − 1} ∀t / ∈ [t j , t j +1 ] q(t) / ∈ P j , where t j denotes the time instant during which the robot passes from polygon P j to polygon P j +1 , i.e., q(t j ) ∈ E j .
Assumptions A0 through A7 were made for two reasons.First, they simplify the description of state constraints used during planning.Second, they help in sound and efficient formulation of the optimization problem solved by the planner.Their conservativeness is often not an issue due to preference for planning safe maneuvers, which should keep the robot sufficiently far from obstacles in many practical scenarios.
Polygon sequence P f ree satisfying assumptions A4-A6, such as the one in Fig. 3, can be obtained from a motion environment map in two stages.First, environment map is decomposed into convex polygons utilizing algorithms such as [47] (for arbitrary polygons) and [14] (for grid-based maps).Second, the generated decomposition can be treated as a graph of polygons, which can be searched using, e.g., Dijkstra's algorithm to find the sequence P f ree .In the next Subsection we will sketch our solution to Problem 1 exploiting a structure of the described environment.

General Description of the Proposed Solution
The planning algorithm proposed in the sequel is designed to generate waypoint sequence W using the controller-driven methodology.Thus, we begin with studying useful properties of the closed-loop system resulting from application of the VFO control law to system (1).Sufficient conditions for motion segment execution without violation of constraint (3) under specific nominal conditions are derived in Section 4.2.After that, Problem 1 is separated into two subproblems: waypoint orientations planning and waypoint positions planning.A method of determining a set of possible waypoint orientation choices and encoding those choices as binary decision variables is proposed in Section 5.2.Subsequently, we show that the remaining problem of waypoint positions planning is linear.Those two problems constitute a single MILP (mixed-integer linear program) with the integer part corresponding to waypoint orientations planning and the continuous linear part corresponding to waypoint positions planning.
In Section 5.4, the simplest case Q f ree = Q is assumed and insights about VFO properties are used Fig. 3 Exemplary environment P f ree defined by a convex decomposition resulting in 3 polygons to formulate waypoint positions planning for a given sequence of waypoint orientations as a MILP.This formulation constitutes the core of our approach.
In Section 5.5, conditions for satisfaction of state constraints given by a single convex polygon are derived and used to extend the MILP to solve Problem 1 for the case P f ree = P f 0 , which corresponds to planning in a single convex polygon.
In Section 5.6, the general case of Problem 1 is treated as a concatenation of planning problems for consecutive polygons in P f ree , which are coupled by additional constraints placing last waypoints in consecutive polygons on the corresponding transition edges E j .
In the next Section, the VFO control law will be recalled and analyzed to an extent necessary for understanding of the proposed planning algorithm.

VFO Controller for the Waypoint-Following Task
For simplicity of subsequent planning algorithm design, let us formulate the VFO control law for the waypoint-following task taken from [34] by expressing all of its components in the (i + 1)-st waypoint coordinate frame as follows: where: where design parameters k a , k p , U 2 > 0 and η i ∈ (0, k p ) are constant, while operator Atan2c (•, •) : R × R → R introduced in [8] corresponds to the continuous version of Atan2 (•, •) function.The orienting control u 1 aligns robot configuration with the convergence vector field , the integral curves of which tend to the (i+1)-st waypoint.This alignment results from tracking of auxiliary orientation θ i+1 ai , which corresponds to minimizing of the auxiliary orientation error e i+1 ai .Convergence vector field h i+1 i can be shaped using relative directing coefficient μ i ∈ (0, 1), which is a normalized version of directing coefficient η i ∈ 0, k p originally defined in [8].As shown in Fig. 2, relative directing coefficient μ i influences a convergence character of orientation θ to waypoint orientation θ di+1 .Motion sense in the i-th motion segment is determined by a decision variable σ i ∈ {−1, 1}, where σ i = −1 forces backward motion while σ i = 1 forces forward motion.Robot longitudinal velocity is determined by velocity profile ρ i , which can be shaped flexibly as stated in [34].Proposed definition ( 9) is designed to ensure that robot will travel through waypoints with prescribed constant velocity U 2 and gradually stop at final waypoint configuration q dN .For brevity, we omit the design of more complex velocity profiles and refer the reader, e.g., to [52] for algorithms computing time-optimal profiles with prescribed intermediate velocities.Design parameter k a can be chosen heuristically as k a = 2 U 2 to make curvature of robot motion independent of chosen U 2 value when e i+1 ai = 0. Relevant stability guarantees and convergence properties of the closed-loop system with the VFO control law in the absence of curvature and state constraints are summarized by the following lemma proven in [34].
waypoint switching strategy given in assumption A2, the application of control law (5) to system (1) yields the guarantees: G1.Robot orientation θ converges exponentially fast to the auxiliary orientation θ ai , which converges to waypoint orientation θ di+1 in the sense θ ai (t) → (θ di+1 mod 2π ) as q(t) → qdi .
G2. Robot position q converges to the assumed neighborhood of waypoint position qdi in finite time, that is, G3. Robot configuration q converges asymptotically to the final waypoint configuration q dN in the sense where T ∈ (0, ∞) is a control time-horizon.
Let us now introduce the notions of nominal and non-nominal conditions utilized in further considerations.If one assumes switching distance = 0 in waypoint switching condition from assumption A2 and perfect tracking of auxiliary orientation θ i+1 ai , i.e., e i+1 ai (t i ) = 0 ⇒ e i+1 ai (t) ≡ 0, one says that the motion execution takes place under nominal conditions.In those conditions, consecutive waypoint positions are reached accurately by the robot.Obviously, nominal conditions are impossible to achieve during the practical motion execution due to sensing noise, imperfect actuation, etc., but they can be very closely approximated.Thus, we will utilize the nominal conditions in the subsequent formal considerations, since they are formally possible.Violation of the nominal conditions will be addressed in Section 6.

Characteristic Properties of the VFO Control System
Recent analysis of closed-loop dynamics with the VFO control law under nominal conditions exposed a number of features useful for motion planning.They will be presented in the form of three main properties.
Property 1 For y i+1 di = 0 application of control law (5) to system (1) under nominal conditions implies that robot position q converges to the (i + 1)-st waypoint position qdi+1 along the integral curve f i+1 i y i+1 , p i given by with where p i is treated a parameter.In case when y i+1 di = 0, the integral curve is a line y i+1 = 0.

Remark 1
The parameter p i in (10) corresponds to such coordinate y i+1 > 0 where f i+1 i y i+1 , p i = 0, that is, it describes a point at which curve (10) crosses the y-axis of the (i + 1)-st waypoint coordinate frame.
Proof Let us assume temporarily that ρ i = hi+1 i .Such an assumption can be made without loss of generality, because an integral curve of the closed-loop system is determined by the value of auxiliary orientation θ i+1 ai given by (7), which (under nominal conditions) does not depend on the velocity profile ρ i .Under nominal conditions, substitution of One can solve (13) by substitution x i+1 = z i y i+1 that is possible only for y i+1 = 0, which in turn imposes y i+1 di = 0. Thus, we will analyze the two cases corresponding to y i+1 di = 0 and y i+1 di = 0, respectively.
In the case of y i+1 di = 0, one has y i+1 (τ i ) = 0 immediately from (12), which means that the integral curve must be y i+1 = 0.
In the case when y i+1 di = 0, one substitutes x i+1 = z i y i+1 in (13) and after some algebra obtains: which can be further simplified using (12) to:

Now one can arrange the variables as follows:
and integrate by sides to obtain where C i = const is an integration constant.After computation of C i from boundary conditions and returning to original variables one arrives at the solution: It can be seen that there are infinitely many pairs x i+1 di , y i+1 di corresponding to the same curve ( 14).Now we will compute p i , such that a pair (p i , σ i ) corresponds to a single integral curve, which is a function of y i+1 , as opposed to time.Let us start by considering the interpretation of p i given in Remark 1.Since p i corresponds to such y i+1 > 0 along the integral curve ( 14) that f i+1 i y i+1 , p i = 0, we look for a τ i corresponding to f i+1 i y i+1 , p i = 0: Substitution of ( 15) into ( 12) yields (11) corresponding to y i+1 satisfying the interpretation of p i .If one eliminates τ i from ( 14) utilizing (12), and once again uses the interpretation of p i from Remark 1 to substitute y i+1 di = p i and x i+1 di = 0 in the resulting expression, then after some algebra the parametrized integral curve (10) is obtained.Observe that the purpose of substitution y i+1 di = p i , x i+1 di = 0 is to change parameters of curve ( 14) from x i+1 di , y i+1 di to p i .

Corollary 1
For every α > 0 the scaling relation ) is satisfied by the integral curve (10).
Proof It is known from Remark 1 and proof of Property 1 that p i is, by construction, a specific value of y i+1 .It means that multiplication of y i+1 in (10) by factor α requires also multiplication of p i , by factor α. Given this observation one can write: which proves the postulated relation.
The easily verified scaling relation described by Corollary 1 is key in our planning approach.Still, to effectively plan robot motion under curvature constraint (3) one must analyze curvature of integral curve (10).Results of this analysis are summarized in the following property.
Property 2 Under nominal conditions and for μ i ∈ (0.5, 1), curvature κi y i+1 , p i of the integral curve where curvature κi y i+1 , p i is given by κi y i+1 ,p i = and point ỹi+1 at which curvature κi y i+1 , p i of curve f i+1 i y i+1 , p i attains its extremum is given by with 2  , 6  , 9  , Proof One can obtain relation (17), corresponding to the curvature of integral curve f i+1 i y i+1 , p i by considering (3) under nominal conditions as follows: After computing θ i+1 ai by time-differentiation of θ i+1 ai given by ( 7), substituting θ i+1 = θ i+1 ai (upon the assumption of nominal conditions) and x i+1 = f i+1 i y i+1 , p i (by virtue of Property 1) into the resulting expression, one applies some algebra to obtain (17).
It can be seen from ( 17) that, similarly to the integral curve (10), for every α > 0 relation holds, because using Corollary 1 one can write κi (αy i+1 , αp i ) (17) = which can be reduced to κi (x i+1 , y i+1 )/α.It is known from Corollary 1 that scaling y i+1 in (10) by a positive factor α corresponds to scaling of p i and f i+1 i y i+1 , p i by the same factor, hence after considering relation (20) one concludes that, for any α > 0, curvature of robot motion along curve (10) with parameter p i multiplied by α corresponds to curvature of robot motion along the original curve (10) divided by α.It means that one can compute the bound K i as given by ( 16) for an integral curve (10) with any p i if the supremum of curvature of motion κi ( ỹi+1 , 1) attained at point ỹi+1 for the specific integral curve (10) with p i = 1 is known.Thus, we will now assume p i = 1 and focus on finding the supremum κi ( ỹi+1 , 1) .To find the supremum κi ( ỹi+1 , 1) , one must first analyze the behavior of κi (y i+1 , 1) as y i+1 → 0. For simplicity, let us consider the limits of the following two auxiliary expressions for the particular case and lim = lim where with the value of κ li for μ i = 0.5 computed under the assumption of 0 0 = 1 as suggested in [7].Using computed auxiliary limits one can write lim y i+1 →0 κi (y i+1 , 1) which in light of scaling relation (20) means that ∀p i lim y i+1 →0 κi (y i+1 , p i ) = 0 for μ i > 0.5.Thus, assuming μ i ∈ (0.5, 1) one concludes that the supremum κi ( ỹi+1 , 1) occurs at the point ỹi+1 where ∂ κi (y i+1 , 1) /∂y i+1 = 0.It remains to compute ỹi+1 by differentiation of curvature relation (17) w.r.t.y i+1 , which yields: where Mi = 0 and Ni are polynomials of y i+1 .Since one can show that Mi = 0, it suffices to find a solution of Ni = 0, which can be obtained by substitution ν i = y i+1 2μ i and explicit solution of resultant cubic polynomial, yielding two conjugate complex roots and one real root ỹi+1 given by (19).Existence of a single real root means that ỹi+1 is exactly the point at which curvature along considered specific integral curve (10) with p i = 1 attains its extremum κi ( ỹi+1 , 1) .Since κi ( ỹi+1 , 1) corresponds to curvature bound K i for the case of p i = 1, one can compute the curvature bound K i for an arbitrary value of p i by applying the scaling relation (20), which yields (16). where Proof From the definition of nominal conditions we know that e i+1 ai ≡ 0, therefore = arctan Along a straight line y i+1 di = a i+1 i x i+1 di one obtains which after taking φ i = tan θ i+1 di , leads to which can be solved w.
sign (φ i ) = sign a i+1 i .Thus, from the two obtained solutions one chooses the one satisfying sign (φ i ) = sign a i+1 i .That solution is (21).
In the sequel we will use properties of the VFO control law proven above to design the VFO-driven motion planning algorithm solving Problem 1.

Formulation of Problem 1 as a General Optimization Problem
In what follows we assume σ i = −sign x i+1 di and θ i+1 di < π/2 to utilize Property 3. Using results from Section 4.2 one can formulate the task of finding a waypoint sequence corresponding to a solution of Problem 1 as the following dynamical nonlinear optimization problem: min subject to constraints: ∀t ∈ [0, T ] q(t) where T ∈ (0, ∞) denotes a control time horizon and R is a 2D rotation matrix.Cost function z i in (22) represents an arbitrary additive cost-to-come measure such as, e.g., predicted robot path length.Differential constraint ( 23) is imposed by assumed kinematic model of a unicycle (cf.( 1)).Since Problem 1 postulates application of a feedback control law, the control input u of system ( 23) is given by ( 24) corresponding to the VFO control law (cf.( 5)).Constraint ( 27) is formulated to guarantee satisfaction of motion curvature constraint (3) under nominal conditions by virtue of Corollary 2. An effective formulation of state constraints given by ( 25) is derived later in Section 5.5.
For now, in anticipation of that formulation we impose an additional constraint (26) to guarantee that Property 3 can be used.Boundary conditions are enforced by equality constraints ( 28) and ( 29), which reflect the requirement that planned waypoint sequence must start and end at prescribed configurations q d0 and q dN , respectively.Observe that constraints ( 28) and ( 29) result from utilization of homogeneous 2D transformations to represent prescribed initial configuration q d0 in terms of prescribed goal configuration q dN and decision variables q i+1 di , which correspond to waypoint configurations expressed in local coordinate frames of subsequent waypoints.
Remark 2 Note that differential constraint (23) imposed by unicycle kinematics is represented in continuous time and robot configuration domains, as opposed to discretized state transition equations assumed in various methods available in the literature.Constraints ( 23) and ( 25) necessitate constraint satisfaction checking in continuous time and robot configuration domains, which is possible thanks to the application of the VFO feedback control law and utilization of its properties developed in Section 4. In our approach, we opted for MILP formulation to perform the optimization in (at least partially) a continuous domain and exploit linearities visible in our optimization problem.It is worth noting that while the discontinuous portion of our MILP model corresponds to discretization of robot orientation values at waypoints, the constraints ( 23)-( 29) are satisfied in a continuous domain (i.e., no discretization is performed) and the planned VFO feedback control policies are also continuous.To the best of our knowledge, straightforward application of global optimization algorithms such as sampling-based or grid-searchbased methods to optimization problem given by ( 22)- (29) would require discretization in time, control or configuration domain, which is incompatible with our problem statement.
For a prescribed upper bound on number N of motion segments, one can attempt to solve the presented nonlinear optimization problem using properties from Section 4 and continuous nonlinear optimization algorithms.To the best of our knowledge this problem is in general non-convex.As a result if a nonlinear optimization solver is directly applied, its solutions in general can be only locally optimal, while the optimization process can be costly and highly dependent on initial guess, which for some optimization algorithms must be a feasible.To show non-convexity of the proposed optimization problem formulation it suffices to notice that even if one assumes convexity of z i q i+1 di and neglects state constraints (25), there remain two constraints violating assumptions of convex optimization problems.First, equality constraint ( 29) is non-linearly dependent on waypoint angles θ i+1 di .Second, after representing (27) in terms of q i+1 di by using (10) and investigating second order partial derivatives of constraint ( 27) w.r.t.θ i+1 di , x i+1 di and y i+1 di , one determines that its feasible set is nonconvex.
To overcome the described issue of non-convexity, we convert the above nonlinear optimization problem to a MILP formulation.The conversion is performed by applying a waypoint orientation discretization and selection algorithm described in Section 5.2.Upon application of this algorithm the number of motion segments N is determined, while constraints (28) and (26) are represented by linear constraints with auxiliary binary decision variables.Subsequently, boundary position constraints ( 29) and curvature constraints (27) are represented as linear constraints in Section 5.4.After that, we give an efficient and exact linear formulation of state constraints (25) represented by a single convex polygon in Section 5.5, which is later generalized to a sequence of convex polygons in Section 5.6.

Discretization and Preparation of a Grid for Waypoint Orientations
The goal of the proposed waypoint discretization algorithm is to generate a waypoint orientation grid, determining a prescribed finite set of waypoint Fig. 5 Exemplary orientation grid and orientation plan for θ d0 = 0.5 rad, θ dN = 1.3 rad, N L = 1, L θ = 1.6 rad, and θ = 0.4 rad.Orientation plan is generated from the orientation grid by degeneration of automatically selected motion segments orientation sequences called waypoint orientation plans, from which the planner will be choosing an optimal one.Given a waypoint orientation grid, one can encode the choice of optimal waypoint orientation plan using a finite number of binary decision variables in the MILP as shown in Section 5.3.Exemplary orientation grids are presented in Figs. 5 and 6.Waypoint orientation θ di is illustrated as a function of motion segment index.Notice that, while the number N of motion segments is fixed, some motion segments can be degenerated as shown in Fig. 5.When the i-th motion segment is degenerated its length is forced to Fig. 6 Exemplary orientation grid for θ d0 = 0.5 rad, θ dN = −1.5 rad, N L = 2, L θ = 1.6 rad, and θ = 0.4 rad 0, which means that during plan execution i-th and (i + 1)-st waypoints will have equal positions and by virtue of assumption A2 the controller will switch instantaneously to the (i + 2)-nd waypoint as if the (i+1)-st waypoint did not exist.Such an instantaneous switch implies that current waypoint orientation will never change to the i-th waypoint orientation given by original waypoint grid as illustrated by difference between the plots of an orientation grid and an orientation plan shown in Fig. 5.An optimal waypoint orientation plan is automatically chosen by degeneration of contiguous subsequences of motion segments contained in the waypoint orientation grid as shown in Figs. 5, 6 and 7.
Assuming an ability to degenerate sequences of motion segments (the technicalities of which are described in Section 5.4) we propose a heuristic approach to generation of a waypoint orientation grid.It shall be generated on the basis of prescribed N L ≥ 0 periods of a triangle wave with amplitude L θ ∈ (0, π), and offset θ d0 (cf.Figs. 5 and 6).Moreover, it shall be quantized with some prescribed resolution θ ∈ (0, π/2).It is assumed that resolution θ is chosen to satisfy (θ dN − θ d0 ) mod θ = 0 and L θ mod θ = 0 so that the zeroth motion segment starts precisely at orientation θ d0 and the last motion segment ends precisely at orientation θ dN .A waypoint orientation grid resulting from such a triangle wave is shown in Fig. 5.It can be seen that in the actual waypoint orientation grid from Fig. 5 every second waypoint orientation is equal to the previous one.Such a redundant choice of waypoint orientations in the grid allows addition of straight-line motion segments (corresponding to the same orientation angles at the ends of the segment) to the generated motion plan, which is of vital practical importance as explained in Remark 4. As a result of the above considerations, we propose the following waypoint orientation grid generation algorithm: SO1.Choose the design parameters: resolution θ ∈ (0, π 2 ), the number of periods N L ≥ 0 and amplitude L θ ∈ (0, π).SO2.Compute the number N of motion segments as follows: SO3. Compute the waypoint orientations in the grid: with where the orientations θ d0 , θ dN are prescribed by the user, while • denotes the integral part of a value (i.e. the value is rounded down to the nearest integer).
The choice of parameters θ , N L and L θ is a trade off between the number of decision variables in the resulting optimization problem and the resolution of waypoint orientation grid explored by the algorithm.This choice can also be determined by specific applications.For example, one can choose large N L (at the expense of a large number of variables in MILP) to allow for potential generation of plans with a large number of robot turns understood as changes in sign of motion curvature.On the other hand, domain-specific knowledge arising from a particular motion scenario (e.g., local planning) could suggest decreasing the values of N L and L θ or even shaping the waypoint orientation grid differently (see Remark 3).In general, thanks to the properties of the VFO control law, acceptable motion plans are often computed with a very small number of waypoints corresponding to high values of θ .That is why we assume in the sequel a high initial value of θ and iteratively decrease its value, which corresponds to solving gradually harder optimization problems.

Remark 3
The strategy of iterative computation of waypoint orientation sequences is just a proposition.Based on particular application, one could devise an entirely different strategy.For example, the waypoint orientation sequence could be randomized or chosen according to another heuristic.Any strategy ensuring that θ decreases over planning time horizon seems to be viable in practice.
Remark 4 Addition of potential straight-line motion segments to the motion plan is a result of practical considerations.Simulation results have shown that in large-scale environments or in the presence of tight state and curvature constraints, one must wait till the resolution θ is close to 0 to obtain a feasible solution due to the lack of straight-line motion planning ability for larger resolution θ .Addition of straight-line segments alleviates this issue by giving the optimizer additional degrees of freedom at the cost of slightly increased problem size (i.e., a number of decision variables).
Remark 5 For simplicity of exposition, the proposed waypoint orientation grid definition assumes fixed constant value of μ i .One can consider a finite set of μ i by adjusting the waypoint orientation grid definition to contain multiple motion segments differing only by their respective μ i values.

General Formulation of the VFO-Driven Motion Planning Strategy
In reference to the considerations from the previous subsection, we propose the iterative motion planning procedure described by Algorithm 1.
The iterative approach from Algorithm 1 allows for relatively fast computation of feasible motion plans optimal for resolution θ with relatively low computational cost, due to the fact that first iterations consider motion plans with a small number of waypoints.
Algorithm 1 VFO-driven motion planning procedure ∀i μ i ← μ, where μ ∈ (0.5, 1) compute N and generate the orientation grid (steps SO1-SO3) [preprocess the orientation grid using Algorithm 2] (optionally) generate and solve the VFO-driven MILP if computed solution cost ≥ c then return VFO-driven MILP solution corresponding to c c ← computed solution cost ñ ← ñ + 2 Algorithm 2 is optionally utilized to reduce the size of orientation grid and improve computational efficiency of the planning algorithm by generation of smaller MILP problems.The algorithm prunes elements of the orientation grid, which are guaranteed to be unreachable.Pruning is carried out in a breadthfirst manner starting from the end of the orientation Algorithm 2 Orientation grid preprocessing procedure mark all elements of the orientation grid as pruned ← {θ dN } while = ∅ do λ ← pop the first element from if curve (10) between q di−1 and q di contained in P j then mark θ di as not pruned and add θ di to remove all elements marked as pruned from the orientation grid grid.Pruning procedure utilizes VFO properties given in the Section 4 and state-constraints checking method described in Section 5.5.The information on how one determines which polygon P j is assigned to a given orientation grid element is provided in Section 5.6.Since a set of possible waypoint orientations is now determined by a waypoint orientation grid, we proceed to waypoint position planning.
We propose to solve the waypoint position planning problem by solving the appropriately formulated MILP.For the sake of clarity, first the general structure of the considered MILP will be given, while further details along with definitions of particular matrices and vectors used in our formulation will be explained gradually in the sequel.The vector of decision variables ξ optimized in the MILP is defined as follows: where varables x i+1 di , i = {0, . . ., N − 1} correspond to x-coordinates of subsequent waypoint positions expressed in local (waypoint) frames, variables γ i ∈ {0, 1} and ζ i ∈ {0, 1} encode choices between forward motion sense (γ i = 1, ζ i = 0), backward motion sense (γ i = 0, ζ i = 1) and segment degeneration (γ i = 0, ζ i = 0) in consecutive motion segments, while variables l i (when minimized) correspond to x i+1 di and have been introduced to model an absolute value function in the considered MILP.Using the introduced variables, the MILP solving Problem 1 is defined as follows: min subject to constraints: , (34) where l comprises the positive weights of the cost vector.The motion cost ( 32) is a sum of partial motion costs for all N segments scaled by a factor (1 + w N N) penalizing high number of motion segments with w N ≥ 0 being a design parameter.The partial motion cost for the i-th segment corresponds to the length of integral curve f i+1 i y i+1 , p i , which is calculated by taking the precomputed length li of integral curve di known from the orientation grid and scaling it by a factor of l i .The matrices F i and vectors b f i contain state constraints corresponding to q ∈ Q f ree , matrices ensure that min l i = x i+1 di .In the context of matrices and vectors, ≤ and ≥ operators denote element-wise operations.We will now systematically derive the constraints used in our problem formulation and represent them in terms of the above matrices and vectors for the cases of unbounded configuration space, state constraints represented by a single convex polygon and state constraints represented by a sequence of convex polygons.
To complete the general description of considered MILP, we will now discuss assumed cost functional (32) in details.As mentioned earlier, it corresponds to the total length of a composite curve resulting from concatenation of N integral curves f i+1 i y i+1 , p i scaled by term (1 + w N N), which essentially penalizes motion plans generated by solution of MILPs with lower values of resolution θ .Since θ i+1 di is determined by waypoint orientation grid, one precomputes the base integral curve length li for all N motion segments numerically in one shot using (10)  One cannot straightforwardly represent an absolute value of decision variable such as x i+1 di in a MILP cost functional such as (32), because MILP requires that the cost functional is a linear combination of optimized decision variables.For the purpose of expressing x i+1 di in the considered MILP, we apply the so-called minimax reformulation and introduce additional variables l i and formulate constraints ensuring that min l i = x i+1 di .Upon the solution of considered MILP, motion cost (32) will be minimized, that is variables l i will attain their minimum values allowed by constraints and one will have l i = x i+1 di .To formulate constraints necessary to ensure that min l i = x i+1 di , one must consider the fact that , which implies that constraints must be satisfied by l i .Since (35) ensure that l i is lower-bounded by max −x i+1 di , x i+1 di , it will converge to the value of max −x i+1 di , x i+1 di upon minimization of motion cost (32) (i.e., upon solution of MILP), thus l i = x i+1 di will be satisfied.Constraints (35) can be transformed to the following matrices and vectors utilized in ( 34) The cost functional ( 32) is designed to ensure that setting w N = 0 will lead to lower total sums of integral curves lengths, that is, shorter motion plans at the expense of higher number of waypoints.High number of waypoints should be avoided if possible due to growth of plan representation and resultant high number of switches between the waypoints during plan exectuion.Note that the value of (1 + w N N) comes into play only when the solution of the current MILP instance is compared with the best solution encountered so far in of the proposed iterative motion planning algorithm.For a single MILP instance (1+w N N) is a precomputed constant, therefore it does satisfy linearity requirements imposed on (32) by definition of a MILP.

Planning in Unbounded Configuration Space
Let us consider the simplest planning scenario, where Q f ree = Q.In that case one considers state constraint matrices F j and transition constraint matrices T j to be zero matrices.The definitions of remaining constraints constitute the core of our MILP formulation in this case, and correspond to constraints ( 27), ( 28) and ( 29) from the general nonlinear problem discussed earlier.
We now direct our attention to the position boundary conditions from Problem 1, which are represented by constraint matrix A g .We assume that once a motion segment is degenerated its length is forced to 0, thus x i+1 di = 0 is forced in this case by constraints discussed later.Therefore, when boundary conditions and state constraints are considered, non-degenerated and degenerated motion segments can be treated in the same manner.Observe that since θ i+1 di is determined by the waypoint orientation grid specified by (31), μ i is determined by choice in Algorithm 1, and since Property 3 holds, one can describe waypoint position qi+1 di only in terms of x i+1 di as y i+1 di = a i+1 i x i+1 di with known a i+1 i .Thus, position constraint ensuring that the zeroth motion segment begins at prescribed position qd0 , while the last motion segment ends at qdN (corresponding to original nonlinear constraint ( 29)) arises naturally and is imposed by matrix-vector pair used in (33): which results from N-number of applications of the waypoint coordinate frame transformations as expressed in (29).
Let us move on to curvature constraints formulation corresponding to (27) where p i (x i+1 di = 1, y i+1 di = a i+1 i ) denotes a specific value of (11) for particular values of x i+1 di , y i+1 di .Substitution of (36) in the constraint (27), which ensures satisfaction of (3) under nominal conditions, yields , (37) as illustrated in Fig. 7.At the same time one must notice that, the constraint imposing x i+1 di = 0 for degenerated segments is contradictory to x i+1 di ≥ x i+1 bi , i.e, constraint (37) cannot be satisfied simultaneously with constraint x i+1 di = 0.That is why we consider curvature constraints corresponding to (3) and segment degeneration mechanism in common constraint inequalities as follows: assuming additional constraint where x i+1 di denotes an admissible upper bound of x i+1 di , taken as an arbitrarily large finite value in this case.Inequality (39) ensures that one of the following three admissible possibilities can be chosen for the ith motion segment: - Observe that when motion segment is degenerated, inequalities (38) correspond to an equality x i+1 di = 0, which forces the length of a degenerated segment to 0. When backward sense of robot motion is chosen, then (38) take the form of Those two conditions correspond to (37).Introduced constraints can be transformed to a matrix representation used in (34) by taking: where 1 N denotes a vector of N-elements, all of which are equal to 1.
Let us turn to the description of orientation boundary conditions.In Section 5.2, it was mentioned that a possibility of degenerating certain motion segments by forcing their length to 0 must be ensured.To utilize Property 3, upon which our planning approach hinges, one must ensure that the waypoint orientation θ dl of the next non-degenerated waypoint after an i-th waypoint satisfies as postulated by waypoint orientation grid (31).Relation (40) holds only for the waypoint orientation plans corresponding to particular configurations of the decision variable vectors γ and ζ .Thus, feasible waypoint orientation plans satisfying (40) must satisfy the following constraints for all motion segments: which result from consideration of ( 40) and the fact that the i-th motion segment is non-degenerated iff γ i = 1 or ζ i = 1.Note that 2L θ < L < ∞ corresponds to an arbitrarily chosen very large value, which influences the above constraints in such a way that they are always satisfied upon degeneration of the segment regardless of other conditions.Introduced constraints guarantee simultaneous satisfaction of (40) and constraint (28) from the general nonlinear optimization problem.One can now straightforwardly define the matrices and vectors used in (34) corresponding to thoseconstraints:

Planning in a Single Convex Polygon
In this subsection the introduced optimization framework is applied to case when Q = Q f ree .Let us assume that Q f ree = P f ree × R is described according to assumption A4 with M = 0 and formulate constraints guaranteeing that robot position will remain in a single convex polygon P f 0 .For convenience, we exploit assumption A6 to represent lines, which contain all E 0 edges of polygon P f 0 as follows: Using such a representation one can write robot position constraints: where β pi ∈ {−1, 1} denotes the sign which can be found by checking which value of sign β pi satisfies inequalities (41) for any chosen point lying inside the polygon P f 0 .To clarify, the sign β pi determines whether all the points lying inside the polygon P f 0 must lie above or below the line Y = c p X + d p .Note that constraints (41) correspond to the constraint (25) and must be satisfied ∀t ∈ [t i , t i+1 ].
Checking (41) ∀t ∈ [t i , t i+1 ] is computationally expensive and intractable to do exactly, therefore we utilize the VFO properties and notice that (under nominal conditions) it suffices to check only 3 points on an integral curve f i+1 i y i+1 , p i to determine the satisfaction of constraints (41) in the i-th motion segment, namely qdi+1 (i.e., the end of the i-th motion segment), qdi (i.e., the beginning of the i-th motion segment), and point si+1 , p i and is such that the tangent to this curve at point s ypi is parallel to the considered p-th polygon edge.It is known that for a given curve f i+1 i y i+1 , p i and p-th polygon edge, there exists at most one such point, because the auxiliary orientation θ i+1 ai , which is tangent to integral curve f i+1 i y i+1 , p i , varies monotonically w.r.t.y i+1 .It means that, if the curve f i+1 i y i+1 , p i satisfies constraint (41) for the p-th polygon edge, then the point si+1 pi is the one lying the closest or the farthest from the line Y = c p X + d p , because for all other points on integral curve f i+1 i y i+1 , p i the vector tangent to this curve has a non-zero projection on a vector orthogonal to the p-th polygon edge.Therefore, if the mentioned 3 points qdi+1 , qdi and si+1 pi satisfy constraint (41) for a p-th polygon edge, then the whole curve f i+1 i y i+1 , p i satisfies this constraint.Point si+1 pi is computed by substitution of the considered p-th polygon edge slope angle into (21), which yields a directional coefficient a i+1 spi of a particular line s i+1 ypi = a i+1 spi s i+1 xpi , the intersection of which with curve f i+1 i y i+1 , p i given by (10) corresponds to spi .Note that in the case of our method, we are only concerned with satisfaction of constraints (41) for a segment of integral curve f i+1 i y i+1 , p i contained between the end of the i-th motion segment qdi+1 and the beginning of the i-th motion segment qdi .Thus, if s i+1 ypi ≥ y i+1 di , then we fix si+1 pi to the value of qi+1 di .This leads to the following relation describing coordinates of the point si+1 pi : with sign ypi , p i , where f i+1 i is given by (10).The geometric interpretation of the point si+1 pi is shown in Fig. 8.
One concludes from the above considerations, that to satisfy constraints (41) for integral curves f i+1 i y i+1 , p i of all N motion segments it is sufficient to ensure that waypoint positions qdi , ∀i ∈ {0, . . ., N} and critical points spi , ∀i ∈ {0, . . ., N} satisfy (41).Those requirements correspond to constraints and respectively.Upon the application of waypoint coordinate frame transformations in a manner similar to the derivation of matrix A g and vector b g representing position boundary conditions in (33), one can Fig. 8 The critical point spi for the p-th polygon edge utilized to efficiently express state constraints during planning represent constraints ( 43) and ( 44) as matrices F i and vectors b f i used in (34).They are given by the following relations: where , and with a i+1 pi = s i+1 ypi /s i+1 xpi , where s i+1 ypi and s i+1 ypi are computed from (42) using the value of p i corresponding to x i+1 di = 1 and y i+1 di = a i+1 i (cf.Property 3).

Planning in Convex Decompositions
The framework developed in Sections 5.2-5.5 scales to the case of convex decomposition under assumption A7 (sequential polygon visting), because it guarantees an existence of a simple mapping between polygons and motion segments.Since in Section 5.2 we assumed N L = 2(M + 1) (cf. the iterative motion planning algorithm), we propose distributing the N L periods of the waypoint orientation grid evenly between the M + 1 convex polygons comprising P f ree .To this aim, we map waypoints jN p /(M+1),j N p /(M+1)+1,. . .,(j+1)N p /(M+1)−1 to polygon P j (cf.definition of N and ( 31)).The remaining last N − N p waypoints shall be mapped to the last polygon P f M .Given this mapping, constraint matrices F i and vectors b f i are computed exactly as in Section 5.5, with constraints (41) computed for a particular polygon from the set P f ree given by the introduced mapping.It remains to ensure that the last waypoint mapped to polygon P j is lying on transition edge E j +1 (cf.assumption A5).This is vital for satisfaction of state constraints, because the method introduced in Section 5.5 does not consider a case of a motion segment spanning two or more convex polygons.Let us denote transition edge index in a given j -th polygon by k > 0 and the index of the last motion segment mapped to this polygon by m.Then, one can write appropriate equality constraint which ensures that the m-th waypoint position lies on an appropriate transition edge.Note that the equality constraint ( 45) is similar to inequality constraint (43).This similarity is reflected in the matrices T j and vector b j used in used in (33).Application of waypoint coordinate frame transformations to (45) yields: The above constraints ensure that the considered waypoint position satisfies the equation of a line containing a transition edge.Those constraints in conjunction with (41), which are also defined for every motion segment, ensure proper placement of the considered waypoint on the transition edge.In practice, one often wants to ensure that the last waypoint mapped to polygon P j is also located sufficiently far from endpoints of the transition edge E j +1 .This is achieved by appropriate modification of coefficients d p in (43) for i = m and p ∈ {k − 1, k + 1}.The modified coefficients d p shall be chosen to make the resultant new virtual transition edge shorter than the original transition E j +1 and contained entirely within it.The particular values of modified coefficients d p can be obtained using simple algebra.
To conclude presentation of the planning algorithm, let us briefly discuss its completeness.Upon consideration of assumption A4, one concludes that there exists a composite curve with absolute curvature bounded by κ b , which connects q d0 and q dN .On the other hand, recall that angular resolution θ decreases monotonically over iterations of the planning algorithm by definition of Algorithm 1. Monotonic decreasing of θ over planning iterations, implies monotonic decreasing of θ i+1 di and x i+1 di → 0 due to growth of motion segments number N. Therefore, over iterations of the planning algorithm the generated plans converge to a sequence of straight-line segments, which can approximate any curve satisfying (3) including the one connecting q d0 with q dN .We argue that since the set P f ree is compact and (by virtue of assumption A4) neither qd0 , nor qdN lie on its boundary, an approximation of considered curve connecting q d0 with q dN is sufficient.In light of that, after a certain number of iterations, θ will attain a value such that the proposed MILP will have at least one feasible solution, computation of which is guaranteed by the MILP solver given an arbitrarily long solution time.

Execution of the Planned Motion in the VFO Control System
So far, we have considered only nominal motion conditions.From now on, we will consider the nonnominal motion conditions admitting e i+1 ai = 0 and > 0. Consideration of e i+1 ai = 0 is motivated by the fact that, since the auxiliary orientation θ ai is defined for all positions q = qdi , any deviation of the initial robot configuration q(0) from the zeroth waypoint q d0 , (i.e., q(0) = q d0 ) is perceived as e i+1 ai (0) = 0. We consider > 0 (cf.assumption A2), because = 0 requires accurate execution of consecutive waypoint positions (i.e., q(t i ) = qdi ), which is not possible in practical motion conditions due to, e.g., imperfect measurements.
Introduction of non-nominal conditions outlined above has various consequences.For example, even if one had e i+1 ai (0) = 0, there would occur e i+1 ai (t 1 ) = 0, because taking > 0 in the switching condition from assumption A2 implies q(t i ) = q di .It means that the i-th motion segment will not be started in the planned robot configuration q di , but in some other point in -vicinity of q di .To account for that effect we introduce 3 modifications, which robustify the proposed motion strategy: 1.As discussed before, polygon representations P j are appropriately shrinked to ensure that planned waypoint sequences result in integral curves f i+1 i y i+1 , p i no closer than to P f ree boundaries.2. Planning is performed with curvature bound κb ψκ b , ψ ∈ (0, 1) to introduce a margin of available motion curvature which can be utilized by the VFO control law to reject possible disturbances.3. Relative directing coefficient μ i is replanned at the time of waypoint switch (i.e., at every time instant t i ) to minimize e i+1 ai (t i ).The replanned value of μ i mentioned in modification 3 is computed from definition of θ i+1 ai (cf.( 7)), by taking μ i := μi : The above value of μi can be used only iff the resulting new i-th motion segment is contained in Q f ree (checked by verifying satisfaction of constraints represented by matrix F i and vector b f i from (34)) and satisfies curvature constraints (checked using Property 2). Figure 9 illustrates the range of θ i+1 di values guaraneteing e i+1 ai (t i ) = 0 thanks to replanning of parameter μ i .This range is highly dependent on robot position and grows rapidly with y i+1 di .
7 Numerical Examples and Experimental Results

Experimental Setup
To verify effectiveness of the proposed method, a series of experiments has been performed on the mobile robot MTracker integrated into the experimental system shown in Fig. 10.MTracker is a small-scale laboratory robot equipped with differential drive (i.e., (   10 to account for actuator limits (see [8] for details of the Velocity Scaling block).

Results
We present exemplary results of simulations and experimental results obtained by application of the proposed motion strategy comprising VFO-driven In case of scenarios S2 and S3, polygons were shrinked for planning to ensure that integral curves f i+1 i y i+1 , p i lie no closer than 0.05 m to boundaries of P f ree .In Figs.11, 12, 13, 14 and 15, the real boundaries of P f ree are shown as black lines.Transition edges are shown as red lines.Green triangles denote q(0), which in case of scenarios S2 and S3 is purposefully different from q d0 (first magenta waypoint) to exemplify robustness of the proposed motion strategy.Red triangles denote q dN , while magenta triangles denote consecutive waypoints planned by the algorithm.Gray paths correspond to the composite curve computed by concatenation of integral curves (10) for each planned motion segment with consideration of the replanned μ i value, which was actually computed on-line during motion execution as described in modification 3. Blue paths correspond to the actual robot position during simulated or experimental motion execution stage.Let us begin by discussing simulation scenario S1 presented in Fig. 11.It was designed to demonstrate the influence of cost functional (32) on the generated motion plans.For scenario S1 motion sense was fixed to forward motion (σ i = 1, ∀i ∈ {0, 1, . . ., N − 1}) to focus on the effect of parameter w N in the generated plans.Results of scenario S1 verify that in case when w N = 0 the shortest Dubins-like path between initial and final configuration is approximated by the waypoint sequence generated using the proposed algorithm.On the other hand, the plan generated with w N = 0.5 contains significantly less waypoints (i.e., the planning algorithm terminated with smaller N) while maintaining a relatively short path composed from integral curves f i+1 i y i+1 , p i .Similar results were consistently produced across various motion scenarios.
Let us now discuss results of scenario S2 shown in Figs.12-13 and scenario S3 shown in Figs.14-15.Planning times for those cenarios are presented in Table 1.In the case of both S2 and S3, the prescribed motion tasks were executed successfully both in simulation and in experiments with a real robot.Execution was successful despite significant displacement between the actual initial configuration q(t = 0) and the prescribed initial configuration q d0 assumed during motion planning.Actual robot path was very close to the one predicted by using (10), even though motion execution was performed under the non-nominal conditions.
In case of scenario S3 one can see in Fig. 15 that curvature bound ψκ b used during the planning stage was significantly exceeded even during simulated execution.It was caused by waypoint switching in the non-nominal motion conditions, which took place at a position q corresponding to the point in which the curve f i+1 i y i+1 , p i has high curvature.This phenomenon was anticipated by introduction of design parameter ψ, which significantly reduces its influence as can be seen from results of scenario S3.Thanks to curvature margin resulting from ψ < 1, the actual curvature bound κ b was not reached during simulations.On the other hand, bound κ b was reached for multiple short periods of time during experimental motion execution for both scenarios S2 and S3.It is mostly due to the discontinuity in control input u at time instants of waypoint switching, which is present even in nominal conditions, because one has θ i+1 ai → 0 as q → qdi and at the same time θ i+2 ai+1 = 0, ∀ qi+2 = 0 by definition (7).Such a discontinuity introduces a disturbance into the control process, which results in control signal peaks.While resolving this issue will be the topic of our future work, it can be seen from experimental results that it is not critical at least for unicycle-like mobile robot kinematics.The disturbance is visible on time plots of both auxiliary orientation error e i+1 ai and orienting conrol u 1 , but it has no significant influence on robot path in the positions space.State constraints are still satisfied in the presence of curvature bound κ b and robot is successfully driven to final configuration q dN .Moreover, auxiliary orientation error e i+1 ai is in many cases driven back to the close vicinity of zero at the time instant of waypoint switching thanks to modification 3.
In Figs.16 and 17 we present the results of waypoint planning for scenarios S4 and S5, respectively.Those scenarios were prepared to show the computational efficiency of the proposed waypoint planning algorithm in more challenging complex motion environments.The environment from scenario S4 contains distributed convex (rectangular) obstacles, while the environment from scenario S5 contains narrow passages and several larger obstacles, which can pose difficulties for planning algorithms.The sequence P f ree of polygons traversed in S4 and S5 has been obtained as follows.First, a binary occupancy grid with resolution 900 × 250 has been processed to detect a non-convex polygon with holes representing the free space.Second, this polygon decomposed into triangles using constrained Delaunay triangulation.Finally, the triangulation structure was searched using Dijkstra algorithm to find a polygon sequence P f ree connecting initial configuration q d0 with final reference configuration q dN .Determination of the polygon sequence took less than 1 ms on the test PC.The planning times achieved for scenarios S4 and S5 have been gathered in Table 1.The planning times were measured on a Linux PC with a Core i7-2600K processor and GUROBI optimization solver.To assess a computational cost of our method, planning times obtained on average for 10 planning runs of RRT* and SST (Stable Sparse RRT; an algorithm dedicated for kinodynamic planning, see [30]) algorithms have been also shown in Table 1.RRT* and SST algorithms were used to plan piecewise-constant open-loop control signals for system (1) under constraint (3) while minimizing the resultant robot path length.The results for RRT* and SST algorithms were obtained using publicly available implementations provided by authors of [30].Final solutions for RRT* and SST algorithms (cf.Table 1) were chosen as first obtained solutions that corresponded to robot paths of length no bigger than 1.05 l * , where l * denotes robot path length resulting from execution of a waypoint sequence obtained at the end of Algorithm 1 under nominal conditions.While in our approach we do not always explicitly look for the shortest robot paths, such a criterion seems to be best suited for comparisons.It can be seen from Figs. 16,17 and Table 1 that planning succeeds in a reasonable amount of time even for quite complex environments if the pruning procedure presented by Algorithm 2 is utilized.If the  pruning procedure is omitted, the large amount of variables and constraints in the MILP problem can significantly increase the computational cost of the planning process.Note that our algorithm reaches initial solutions faster than considered sampling-based algorithms the case studies represented by scenarios S4 and S5.It seems to be the of analytic constraints satisfaction checking and exploitation of environment structure provided by a convex decomposition.A similar trend is visible for the optimized solutions.Although the presented results allow concluding that the computational cost of our approach can be acceptable, the differences between computational costs of the respective can depending on many factors such as, e.g., environment topology.must consider sampling-based algorithms are more to (as in, [21]) than MILP-based methods as

Final Remarks
The motion proposed experimentally in paper enables of the methodology complex scenarios, which require imposition constraints robot state.controller-driven motion was to exploit specific beneficial of VFO trol law described in Section 4.2.Such an approach fits into the current trend of complementing analytical methods known from control theory with purely algorithmic methods traditionally utilized in computer science.As predicted in [29], this trend leads to establishment of a new area of the so called algorithmic control theory.
Thanks to the controller-driven design methodology, motion plans computed by the proposed planning algorithm can be immediately executed with the VFO control law, and thus the complexity accompanying otherwise necessary post-processing procedures (e.g., proper trajectory parametrization and determination of distance to a reference path) was avoided.A plan represented by a waypoint sequence is simple, easy to store and can be easily modified by human users.Moreover, the waypoint positions are planned in a continuous domain, so that various issues inherent for discrete planning are alleviated.It is also worth noting, that the controller-driven approach allowed for analytically proven, exact and relatively efficient checking of state and curvature constraints crucial in many practical applications.It is also worth noting that our approach seems to be quite flexible.The motion sense (i.e., the decision between forward and backward robot motion) for consecutive motion segments is automatically determined by the planner.It can also be chosen by the user for one or more motion segments.The waypoint orientation grid generation algorithm from Section 5.2 is another flexible component of the proposed motion strategy.It can be substituted with a probabilistic algorithm, an application-specific heuristic approach (e.g. for zig-zag parallel parking maneuvers planning) or a higher-level motion planner.Similarly, the MILPbased formulation of the planning problem empowers the designer with an ability to extend our method with additional constraints in an organized manner.
Presented results have opened several research avenues currently investigated by the authors.One of them concerns the most severe limitation of the VFO motion execution algorithm, which is related to the discontinuity in control input u at the time instants of waypoint switching.It seems that this issue could be solved by an appropriate on-line interpolation between convergence vector field hi of subsequent motion segments.Proposed method could be also extended by further exploitation of convenient polygonal environment representation and linear dependency of various useful motion plan characteristics on assumed optimization variables.For example, one could design the motion cost utilized in optimization with consideration of how far away from obstacles should the robot be kept or how fast is curvature of robot motion changing over time.Furthermore, application of the presented approach to more complex robot kinematics such as car-like and N-trailer robots using results from [35] and [33], respectively, is possible.

Fig. 1
Fig. 1 Considered curvature-constrained kinematic unicycle.Red circles denote paths of maximum admissible curvature κ b

Fig. 2
Fig. 2 Paths of a unicycle driven by the VFO controller under nominal conditions during i-th motion segment execution for two values of μ i coefficient.(note: hi+1 i and vi+1 i have been illustrated for k p = 1)

Corollary 2
Under nominal motion conditions, constraint (3) is satisfied in the i-th motion segment iff qi+1 di is such that |p i | ≥ p bi , where p bi = κi ( ỹi+1 , 1) /κ b (cf.(16)).Corollary 2 results from substitution of K i = κ b in (16) to find a boundary value of p i denoted by p bi .It is graphically illustrated in Fig. 4. The sets of admissible integral curves shown in blue and magenta, respectively, have the borders, which are integral curves (10) evaluated for p bi .While Properties 1 and 2 along with Corollary 2 are strong motion planning tools, there is another useful characteristic of the VFO control law anticipated by Corollary 1 and captured by the following property.Property 3 Under the nominal conditions, let us assume σ i = −sign x i+1 di , and θ i+1 di < π/2.Then for a given waypoint orientation θ i+1 di the position qi+1 di of the i-th waypoint lies on a line r.t. a i+1 i yielding one positive and one negative solution.The choice of the appropriate solution is possible thanks to the assumptions of σ i = −sign x i+1 di , and θ i+1 di < π/2, which ensure that the integral curve f i+1 i y i+1 , p i is contained entirely in a single quadrant of coordinate frame {i + 1} (cf.Figs. 4 and 7 for graphical interpretation).It means that given sign θ i+1 di and σ i one can clearly determine the appropriate quadrant of coordinate frame {i + 1} and the value of sign a i+1 i for segments contained in this quadrant.Under the assumption of θ i+1 di < π/2, one concludes that

Fig. 4
Fig.4 Integral curves(10) for q di = 0 with backward-(left-hand side) and forward (right-hand side) motion sense.Blue and magenta curves have absolute values of curvature lower than κ b .Therefore, under nominal conditions, if initial position qi+1 di lies in blue or magenta region, then constraint (3) is satisfied within the i-th motion segment

Fig. 7
Fig.7 The relation between boundary integral curves (i.e., the ones evaluated for p bi ) and a line of waypoint positions corresponding to a constant waypoint orientation.If the i-th waypoint is placed on this line and x i+1 di ≥ x bi , curvature constraint(3) is satisfied under nominal conditions K 1 through K 8 and vectors b k1 through b k3 impose curvature bounds, while A g along with b g handle position boundary conditions, and A θ 1 , A θ 2 along with b θ 1 , b θ 2 handle orientation boundary conditions.Matrices T j paired with vectors b j for j ∈ {0, . . ., M − 1} ensure that appropriate waypoints are placed on transition edges of consecutive polygons in P f ree .Matrices A l1 through A l4 with vectors b l1 b l2

and assuming x i+1 di = 1 .
It is easy to verify that the term li x i+1 di represents the true length of integral curve f i+1 i y i+1 , p i length, because of linear scaling of integral curve length w.r.t.x i+1 di , which can be established by virtue of Property 1 and Corollary 1.

Fig. 11
Fig. 11 Results of simulation scenario S1.Motion plan visible on top of the figure was generated with w N = 0.5, while plan visible on the bottom was generated with w N = 0.It can be seen that plan generated with w N = 0 accurately approximates the shortesst Dubins-like path at the expense of high number of waypoints

Fig. 12
Fig.12 Robot path and curvature of motion during simulated (left-hand side) and experimental (right-hand side) motion execution of scenario S2

Fig. 13
Fig. 13 Control signals and errors during simulated (left-hand side) and experimental (right-hand side) execution of scenario S2

Fig. 14
Fig. 14 Robot path and curvature of motion during simulated (left-hand side) and experimental (right-hand side) motion execution of scenario S3

Fig. 15
Fig. 15 Control signals and errors during simulated (left-hand side) and experimental (right-hand side) execution of scenario S3

Fig. 16
Fig.16 Results of simulation scenario S4.Motion plan visible on top of the figure was the first feasible solution computed by the planner, while plan visible on the bottom is an optimal solution for w N = 0.5.Design parameters μ i = 0.65 and ψκ b = 2 m −1 were chosen in this case.Green triangle denotes initial configuration, while red triangle denotes prescribed final configuration

Fig. 17
Fig. 17 Results of simulation scenario S5 with an optimal solution for w N = 0.5.Design parameters μ i = 0.65 and ψκ b = 2 m −1 were chosen in this case.Green triangle denotes

Table 1
Planning times for scenarios S2-S5.No planning limit was assumed.The times obtained when pruning with 2 shown parentheses Scenario to 1-st [s] Time end of [s] RRT* 1-st sol.[s] final sol.[s] SST 1-st sol.[s] SST final sol.[s]