A Nonlinear Constrained Optimization Framework for Comfortable and Customizable Motion Planning of Nonholonomic Mobile Robots – Part I

# A Nonlinear Constrained Optimization Framework for Comfortable and Customizable Motion Planning of Nonholonomic Mobile Robots – Part I

## Abstract

In this series of papers, we present a motion planning framework for planning comfortable and customizable motion of nonholonomic mobile robots such as intelligent wheelchairs and autonomous cars. In this first one we present the mathematical foundation of our framework.

The motion of a mobile robot that transports a human should be comfortable and customizable. We identify several properties that a trajectory must have for comfort. We model motion discomfort as a weighted cost functional and define comfortable motion planning as a nonlinear constrained optimization problem of computing trajectories that minimize this discomfort given the appropriate boundary conditions and constraints. The optimization problem is infinite-dimensional and we discretize it using conforming finite elements. We also outline a method by which different users may customize the motion to achieve personal comfort.

There exists significant past work in kinodynamic motion planning, to the best of our knowledge, our work is the first comprehensive formulation of kinodynamic motion planning for a nonholonomic mobile robot as a nonlinear optimization problem that includes all of the following – a careful analysis of boundary conditions, continuity requirements on trajectory, dynamic constraints, obstacle avoidance constraints, and a robust numerical implementation.

In this paper, we present the mathematical foundation of the motion planning framework and formulate the full nonlinear constrained optimization problem. We describe, in brief, the discretization method using finite elements and the process of computing initial guesses for the optimization problem. Details of the above two are presented in Part II (Gulati et al., 2013) of the series.

## 1 Introduction

Autonomous mobile robots such as intelligent wheelchairs and autonomous cars have the potential to improve the quality of life of many demographic groups. Recent surveys have concluded that many users with mobility impairments find it difficult or impossible to operate existing power wheelchairs because they lack the necessary motor skills or cognitive abilities (Fehr et al., 2000; Simpson et al., 2008). Assistive mobile robots such as smart wheelchairs and scooters that can navigate autonomously benefit such users by increasing their mobility (Fehr et al., 2000). Autonomous cars have the potential to increase the mobility of a significant proportion of the elderly whose driving ability is reduced due to age-related problems (Silberg et al., 2012).

The motion of an autonomous mobile robot should be comfortable to be acceptable to human users. Moreover, since the feeling of comfort is subjective, different users should be able to customize the motion according to their comfort. Motion planning is a challenging problem and has received significant attention. See  (Latombe, 1991; Hwang and Ahuja, 1992; Choset et al., 2005; LaValle, 2006, 2011a, 2011b). However, most of the existing motion planning methods have been developed for robots that do not transport a human user and issues such as comfort and customization have not been explicitly addressed.

In this paper we focus on planning comfortable motion for nonholonomic mobile robots such that the motion can be customized by different users. Our key contributions are as follows:

• We model user discomfort as a weighted cost functional. This is informed by studies of human comfort in road and railway vehicle literature that indicate that human discomfort increases with the magnitude of acceleration and jerk and that comfortable levels of these quantities have different magnitudes in the direction of motion and perpendicular to the direction of motion (Suzuki, 1998). Thus, our cost functional is a weighted sum of the following three physical quantities: total travel time, tangential jerk, and normal jerk.

Minimum jerk cost functionals have previously been used in literature (Žefran, 1996; Arechavaleta et al., 2008) for optimal motion planning. What is new here is the separation of tangential and normal components, and computing the weights using the technique of dimensional analysis (Langhaar, 1951) that allows us to develop a straightforward procedure for varying the weights for customization.

• We develop a framework for planning comfortable and customizable motion. Here, we present a precise mathematical formulation of kinodynamic motion planning of a nonholonomic mobile robot moving on a plane as a nonlinear constrained optimization problem. This includes an in-depth analysis of conditions under which the cost-functional is mathematically meaningful, analysis of boundary conditions, and precise formulation of constraints necessary for motion comfort and for obstacle avoidance. To the best of our knowledge, such a formulation is absent from the literature.

The idea of computing optimal trajectories that minimize a cost functional is not new and has been used for planning optimal trajectories for wheeled robots (Dubins, 1957; Reeds and Shepp, 1990; Balkcom and Mason, 2002; Bianco and Romano, 2005) and manipulators (Fernandes et al., 1991; Shiller, 1994; Žefran, 1996; Arechavaleta et al., 2008). All of these formulations make several limiting assumptions, such as known travel time, or known path, or boundary conditions on configuration but not its derivatives. None of these approaches consider obstacles. The closest existing work to ours in terms of problem formulation and numerical solution method is (Žefran, 1996), but obstacle avoidance constraints are not part of this formulation.

The trajectories planned by our framework have several useful properties – they exactly satisfy boundary conditions on position, orientation, curvature, speed and tangential acceleration, satisfy kinematic and dynamic constraints, and avoid obstacles while minimizing discomfort. Further, our framework is capable of planning a family of trajectories between a given pair of boundary conditions and can be customized by different users to obtain a trajectory that satisfies their comfort requirements.

• We represent obstacles as star-shaped domains with piecewise boundary. This choice allows treatment of non-convex obstacles without subdividing them into a union of convex shapes. This reduces the number of constraints imposed due to obstacles and leads to a faster optimization process. Such a representation of obstacles is not very common in robotics where most collision-detection algorithms assume polygonal obstacles, and detect collisions between non-convex polygons by subdividing them into convex polygons (Quinlan, 1994; Mirtich, 1998; Lin and Manocha, 2004).

• We use the Finite Element Method to discretize the above infinite-dimensional problem into a finite dimensional problem. The finite element method is not unknown in trajectory planning but it is not very common. However, it is a natural choice for problems like ours and we strongly believe that using it provides us insight, flexibility, and reliability that is not easily obtained by choosing other discretization methods.

• Our method can be used independently for motion planning of nonholomic mobile robots. It can also be a used as local planner in sampling-based methods (LaValle, 2006) since the trajectories computed by our method exactly satisfy boundary conditions, kinodynamic constraints, continuity requirements, and avoid obstacles.

## 2 Background and related work

In this section, we characterize motion comfort by analyzing studies in ground vehicles, elevator design, and robotics. We then review existing motion planning methods and identify their strengths and limitations in planning comfortable motion.

### 2.1 Comfort

Comfort - What is it? Comfort has both psychological and physiological components, but it involves a sense of subjective well-being and the absence of discomfort, stress or pain  (Richards, 1980).

Studies to characterize comfort in ground vehicles such as automobiles and trains have shown that the feeling of comfort in a vehicle is affected by various characteristics of the vehicle environment including dynamic factors (such as acceleration and jerk), ambient factors (such as temperature and air quality), and spatial factors (such as seat quality and leg room) (Richards, 1980). In this work we focus on comfort due to dynamic factors alone.

Passenger discomfort increases as the magnitude of acceleration increases (Suzuki, 1998; Jacobson et al., 1980; Pepler et al., 1980; Förstberg, 2000; Chakroborty and Das, 2004). This is because an increase in magnitude of acceleration implies increase in magnitude of force experienced by a passenger. Two separate components of acceleration effect discomfort – tangential component along the direction of motion and normal component perpendicular to the direction of motion (Jacobson et al., 1980; Pepler et al., 1980; Förstberg, 2000). The normal component is zero in a straight line motion but becomes important when traversing curves. The actual values of comfortable bounds of the two components may be different (Suzuki, 1998), may vary across people, may depend on the mode of transportation, and may depend on the passenger’s position (Pepler et al., 1980; Förstberg, 2000). Hence, guidelines for ground transportation design prescribe maximum values of accelerations (Suzuki, 1998; Chakroborty and Das, 2004; Iwnicki, 2006), or maximum values of comfort indices that are functions of accelerations (ISO, 1997; CEN, 1999).

Discomfort also increases as the magnitude of jerk increases (Pepler et al., 1980; Förstberg, 2000). This is because a high rate of change of jerk implies a high rate of change of magnitude or direction or both of the forces acting on the passenger. Upper bounds on jerk for comfort have been proposed for road (Chakroborty and Das, 2004) and railway vehicles (Suzuki, 1998). In elevator design, motion profiles are designed for user comfort by choosing profiles with smooth accelerations and low jerk (Hall et al., 1970; Krapek and Bittar, 1993; Spielbauer and Peters, 1995).

From a geometric standpoint, it has been known for more than a century that sharp changes in curvature of roads and railway tracks can be dangerous and can cause passenger discomfort (Laundhart, 1887; Glover, 1900; Lamm et al., 1999). For a point mass moving on a path, the normal acceleration at a point is given by where is the curvature of the path and is the speed at that point. If curvature is not continuous, then normal acceleration cannot be continuous unless the speed goes to zero at the point of discontinuity. This is clearly undesirable for comfort. In robotics, the desire to drive a robot with non-zero speed from start to goal has led to the development of methods for planning continuous curvature paths  (Lamiraux and Laumond, 2001; Fraichard and Scheuer, 2004; Bianco and Romano, 2004, 2005; Piazzi et al., 2007).

To summarize, in a motion planning context, a trajectory should have the following properties for comfort. First, the acceleration should be continuous and bounded. Second, jerk should be bounded. Third, the geometric path should have curvature continuity so that is is possible to travel from start to end without stopping. Fourth, a trajectory should exactly satisfy appropriate end point boundary conditions boundary conditions on position, orientation, curvature, speed, and acceleration since many tasks require precise these (for example, positioning at a desk for an intelligent wheelchair, parking in a tight parking space for a car). Fifth, it should be possible to join multiple trajectories such that the combined trajectory has the above properties. This means that a trajectory should satisfy the above described boundary conditions on both ends.

### 2.2 Motion planning

There exists a large body of work on motion planning. Before reviewing this work, we define some terms. The space of all possible positions and orientations of a robot is called configuration space. The space of all possible configurations and their first derivatives is called state space. A trajectory is a time-parameterized function of configuration. A control trajectory is a time-parameterized function of control inputs.

Motion planning is the problem of finding either a trajectory, or a control trajectory, or both, given the initial and final configuration, and possibly their first and higher derivatives, such that the geometric path does not intersect any obstacles, and the trajectory satisfies kinematic and dynamic constraints. Kinematic constraints refer to constraints on configuration and dynamic constraints refer to constraints on velocity and its higher derivatives. These constraints arise from physics, engineering limitations, or comfort requirements.

A variety of methods have been used to solve various aspects of the motion planning problem. Path Planning methods focus on the purely geometric problem of finding a collision-free path. Another set of methods, stemming from differential geometric control theory, focus on computing control inputs that steer a robot to a specified position and orientation or that make a robot follow a specified path. Kinodynamic motion planning methods, consider both dynamics and obstacles and focus on computing collision-free trajectories that satisfy kinematic and dynamic constraints. See  (Hwang and Ahuja, 1992; Latombe, 1991; Choset et al., 2005; LaValle, 2006) for excellent presentation of all three kinds of methods, (Laumond et al., 1998) for differential geometric control methods, and (Donald et al., 1993; Fraichard, 1996; LaValle and Kuffner, 2001a; Hsu et al., 2002) for kinodynamic planning. In this work, we use motion planning in the sense of (Donald et al., 1993), that is, we speak of kinodynamic motion planning, consistent with the informal definition presented above.

#### Sampling-based methods

Sampling-based methods have found widespread acceptance and practical use for motion planning. These methods are used for both path planning (LaValle, 1998; Kavraki et al., 1996; LaValle and Kuffner, 2001b) and for motion planning (Canny, 1988; Barraquand and Latombe, 1989; Donald et al., 1993; Fraichard, 1996; LaValle and Kuffner, 2001b, a; Hsu et al., 2002). See (LaValle, 2006) for an in-depth discussion. The main idea in all sampling-based methods is to sample the state space (Donald et al., 1993; LaValle and Kuffner, 2001a) or state-time space (Erdman and Lozano-Pérez, 1987; Barraquand and Latombe, 1990; Fraichard, 1996; Hsu et al., 2002) to construct a directed graph called a roadmap from the start state to the goal region. The vertices of this graph are points in the obstacle free region of the appropriate space (state space or state-time space) and the edges are trajectory segments that satisfy kinodynamic constraints. The sequence of control inputs associated with the edges of the roadmap is the control trajectory. Among the most computationally efficient methods here are the ones that add vertices to the graph by randomized sampling.

Randomized sampling-based algorithms follow two paradigms – multiple-query and single-query. In the multiple-query paradigm, a roadmap is constructed once and used to answer multiple path planning queries. These algorithms are particularly computationally efficient in an unchanging environment since a single roadmap can be used to answer multiple queries. Some of the most well-known algorithms that follow this paradigm are Probabilistic Roadmaps (PRMs) and its variants (Kavraki et al., 1996).

The closeness of the end point of the trajectory to the goal state increases as the resolution increases, but in general, it is not possible to find a trajectory that exactly reaches the goal state. If it is desired to reach a goal state exactly, then a boundary value problem has to be solved between the end state of the solution trajectory and the goal state. This is a non-trivial problem since the solution must avoid obstacles and satisfy kinodynamic constraints. Some sampling-based methods are bidirectional, that is, they simultaneously grow roadmaps from the start state as well as the goal state. In this case, a solution trajectory exactly satisfies the boundary conditions. However, like before, a boundary value problem has to be solved to connect the two roadmaps.

Since a fixed value of control input is applied for a finite length of time at each step, the planned path lacks curvature continuity and has to be smoothed in a post-processing step. Curvature continuity can be attained at the cost of increasing the dimensionality of the state space, and has been demonstrated only for a path planning problem (Scheuer and Laugier, 1998). Similarly, for achieving acceleration continuity the dimensionality of the state space has to be increased resulting in increased computational complexity.

Recently, sampling-based algorithms described above have been shown to almost always converge to solution that has non-optimal cost (Karaman and Frazzoli, 2011) and a new algorithm, RRT* was proposed for planning asymptotically optimal paths. Results in a two dimensional configuration space showed that algorithm is computationally efficient. While promising, these results are very recent, and extending this work to kinodynamic motion planning is yet to be carried out.

Another set of sampling-based methods can compute optimal trajectories by constructing a grid over the state space or state-time space and searching this discrete grid using graph-search algorithms such as A* (Canny, 1988; Barraquand and Latombe, 1989; Fraichard, 1996). This grid is called the state-lattice. Each pair of neighboring vertices of the grid are connected to each other by a trajectory that satisfies kinodynamic constraints. Three key choices effect the solution quality. First, the choice of discretization determines the closeness of the solution to the true optimum and the speed of computing the solution. Second, the choice of a neighborhood (e.g. k-nearest) for a vertex determines the connectivity of the space. Third, the choice of a method for computing trajectory segments between vertices determines the quality of the solution trajectory. Computing trajectory segments between adjacent states involve solving a non-trivial boundary value problem. For continuity of curvature, velocity and acceleration between connected trajectory segments, the state space should include curvature, and the first and second derivative of configuration. This results in increase in dimensionality of the search space and hence increase in computational time. For this reason, lattice-based methods have been shown to plan trajectories, with some but not all of the properties necessary for comfort (Section 2.1) in autonomous driving applications. Continuous curvature trajectories are demonstrated in (Pivtoraiko et al., 2009), continuous velocity but not continuous curvature trajectories are demonstrated in (Likhachev and Ferguson, 2009). Trajectories with continuous curvature, speed, and acceleration are demonstrated in (McNaughton et al., 2011) Here the problem is tractable because the sampling can be restricted to the road on which the vehicle drives. Efficiently planning trajectories that satisfy all properties of comfort as described in Section 2.1 in less structured environments very much remains an open problem.

#### Optimal-control based methods

The problem of planning trajectories that are optimal with respect to some performance measure and also avoid obstacles has been shown to very hard (Canny and Reif, 1987), even in relatively simple cases. However, for many applications, we do require that a solution trajectory be optimal with respect to some performance measure such as time, path length, energy etc.

Optimal control methods (Bryson and Ho, 1975; Troutman, 1995) have traditionally been used for computing optimal trajectories for systems subject to dynamic constraints in the absence of obstacles and have been widely applied in aerospace engineering and control-systems engineering. The formulation consists of constructing a cost functional representing the cumulative cost over the duration of motion and minimizing the cost functional to find a desired state trajectory or control trajectory or both. A functional is an operator that maps a function to a real or complex number.

Sufficient conditions for a solution of the minimization problem are given by the Hamilton-Jacobi-Bellman (HJB) equation. HJB is a second-order partial differential equation with end-point boundary conditions. Analytic solutions of the HJB equation for linear systems with quadratic cost have long been known (Bryson and Ho, 1975). For general nonlinear systems, the HJB equation has to be solved numerically.

Necessary conditions for optimality are derived using Pontryagin’s principle and consist of a set of first-order ordinary differential equations. These differential equations convert the optimization problem into a two-point boundary value problem. The system of differential equations can either be solved analytically (where possible) or numerically using methods such as the shooting method or finite-difference methods.

Analytical solution to the problem of finding minimum length paths for Dubins (Dubins, 1957) car and Reeds and Shepp (Reeds and Shepp, 1990) car (see (Souères and Boissonnat, 1998)) was found using such an approach. Dubins car is only allowed to move forward while Reeds and Shepp car is also allowed to move backward. These paths are comprised of straight line and arc segments and minimize the distance traveled by the mid-point of the rear axle. Each path segment is traversed at a fixed speed, so the trajectories corresponding to these paths are also time-optimal for a given speed. More recently, shortest paths for a differential drive wheeled robot were developed by including a rotation cost in the cost functional (Balkcom and Mason, 2002) (since a differential drive robot can turn in place). Such minimum-time paths lack curvature continuity and require frequent stopping and reorienting of wheels.

More complex problems generally require a numerical solution. One frequently used numerical method is the shooting method where the two point boundary value problem is converted into an initial value problem. Shooting methods have been used for trajectory planning for nonholonomic mobile robots (Howard and Kelly, 2007; Ferguson et al., 2008). However, in shooting methods, it is challenging to specify a good initial guess of the unknown parameters that produces a final state reasonably close to the specified state. In general, the trajectories computed do not exactly satisfy end point boundary conditions.

Instead of solving the differential equations representing necessary conditions, approximation methods that discretize the infinite-dimensional problem into a finite-dimensional one and optimize the cost functional directly in this finite-dimensional space can be used. Such methods have been used for planning optimal trajectories of robots. In (Fernandes et al., 1991), control inputs that minimize total control energy to travel between a given pair of boundary states are computed. Here Fourier basis functions are used for discretization. In (Žefran, 1996), trajectories that minimize the integral of square of norm of end-effector jerk and the square of norm of time derivatives of joint torque vector, subject to torque constraints, are computed. Here a finite-element discretization is used. Other discretizations are also possible, such as B-spline (Bobrow et al., 2001) and spectral (Strizzi et al., 2002) discretization.

Very few of the existing optimal control approaches include obstacle-avoidance. Not only do obstacle avoidance constraints make the optimal control problem highly nonlinear, but also each obstacle divides the set of feasible solutions into disjoint regions. One of the earliest methods that included dynamic constraints and obstacle-avoidance for motion planning of autonomous vehicles used a two step approach – first an obstacle free path was found and then an optimal speed on this path was computed (Shiller and Dubowsky, 1991; Shiller and Gwo, 1991). Because of path-velocity decomposition, the resulting trajectory is, in general, not optimal. Obstacles were included as hard constraints for a two-dimensional translating robot in (Tominaga and Bavarian, 1990).

#### Learning methods

Optimal control methods require an accurate model of the kinematics and dynamics of the robot as well as models of the robot’s interactions with the world. Such models are not always available. Further, it is not straightforward to develop an appropriate cost functional for a given task. Even if such models and cost functionals are available, searching through the high dimensional configuration space of the robot (e.g. in the case of humanoid robots) for an optimal trajectory can be computationally expensive. One set of learning-based methods use the key observation that, in practice, robot trajectories are restricted to a manifold by the task and by the kinodynamic constraints. The dimension of this manifold is, in general, lower than the dimension of the configuration space. These methods aim to learn the structure of this manifold from observed data of the robot’s movement (Ramamoorthy and Kuipers, 2008). Another set of methods aim to learn motion primitives for a specific task using observed data from human movements (Schaal et al., 2003). A detailed discussion of these methods is beyond the scope of this work and the interested reader is referred to the following works for more details: (Full and Koditschek, 1999; Schaal et al., 2003; Calinon and Billard, 2009; Ramamoorthy and Kuipers, 2008; Havoutis, 2012).

#### Summary

Trajectories computed by sampling-based methods, in general, lack continuity of curvature and acceleration. While these problems can be solved by increasing the dimensionality of state space at the cost of increased computational complexity, the problems of lack of optimality and not satisfying the goal boundary conditions exactly still remain.

Optimal control methods have primarily been demonstrated for trajectory planning in the absence of obstacles. Further, a comprehensive formulation of kinodynamic motion planning problem for nonholonomic mobile robots that includes obstacle avoidance is absent. Thus, none of the existing methods can be directly applied to planning comfortable and trajectories. To this end, we develop a motion planning framework to compute trajectories that result in comfortable motion.

## 3 Overview of the approach

At the root of our framework is the assumption that user discomfort can be quantified as a cost functional, and that trajectories that minimize this discomfort and avoid obstacles will result in user-acceptable motion. We outline the main steps of our approach below.

• Formulate user discomfort as a mathematically meaningful cost functional. Based on existing literature, and making the assumption that a user would like to travel as fast as is consistent with comfort, we define a measure of discomfort as a weighted sum of the following three terms: total travel time, time integrals of squared tangential jerk and squared normal jerk.

Each weight used in the discomfort measure to add different quantities is the product of two factors. The first factor has physical units so that the physical quantities with different dimensions can be added together. It is a fixed function of known length and velocity scales. The second factor is a dimensionless parameter that can be varied according to user preferences. The dimensional part is derived using the standard technique of dimensional analysis (Langhaar, 1951).

• Define the problem. We formulate our motion planning problem as follows: “Given the appropriate boundary conditions, kinodynamic constraints, the weights in the cost functional, and a representation of obstacles, find a trajectory that minimizes the cost functional, satisfies boundary conditions, respects constraints, and avoids obstacles”. This description is transformed into a precise mathematical problem statement using a general nonlinear constrained optimization approach.

• Choose a parameterization of the trajectory. Mathematically, one can use different functions to fully describe a trajectory. We express the trajectory by an orientation and a velocity as functions of a scaled arc-length parameter where the scaling factor is an additional scalar unknown to be solved for. This leads to a relatively simple expression for discomfort. We use a scaled arc-length parameterization Thus, we do not assume that the path length is known until the problem is solved.

• Analyze the boundary conditions. A complete analysis of boundary conditions shows that for the optimization problem to be well-posed, we need to impose boundary conditions on position, orientation, curvature, speed, and tangential acceleration on each end. Further, we find that three different types of boundary conditions on speed and tangential acceleration on each end describe all types of motion tasks of interest such as starting/ending at rest or not.

• Choose a representation of obstacles. To incorporate obstacle avoidance, we make the assumption that each obstacle can be modeled as a star-shaped domain with a boundary that is a piecewise smooth curve with continuous second order derivative. If an obstacle is not star-shaped, our framework can still handle it if it can be expressed as a finite union of piecewise smooth star-shaped domains. It is assumed that a representation of each obstacle is known in polar coordinates where the origin lies in the interior of the kernel of the star-shaped domain. Since each obstacle is assumed star-shaped, the constraint that the trajectory stay outside obstacles can be easily cast as an inequality.

To efficiently incorporate obstacle avoidance constraints, we have to introduce position on the path as an additional unknown. This leads to a sparse Hessian of constraint inequalities, which otherwise would be dense. The position as an unknown is redundant in that it can be computed from the two primary unknowns (orientation and speed). Hence that relation is included as an extra equality constraint.

• Discretize the problem. We use finite elements to convert the infinite-dimensional minimization problem to a finite dimensional one. For discomfort to be mathematically meaningful and bounded, both speed and orientation must have square-integrable second derivatives. We use a uniform mesh and cubic Hermite polynomial shape functions on each element for speed and orientation. Starting or stopping with zero speed is a special case that requires that speed have an infinite derivative (with respect to scaled arc-length) with a known strength on the corresponding boundary point. In this case we use singular shape functions for speed only on elements adjacent to the corresponding boundary.

In the non-discretized version of the optimization problem the obstacle avoidance constraint can be expressed as the condition that each point on the trajectory should be outside each obstacle. We discretize this into a finite dimensional set of inequalities by requiring that some fixed number of points on the trajectory be outside each obstacle.

• Compute an appropriate initial guess. A good initial guess is necessary for efficiently solving any nonlinear optimization problem. In general, there exist infinitely many trajectories between any given pair of boundary conditions. Based on our analysis of this non-uniqueness, we compute a set of four good quality initial guesses by solving another, simpler, optimization problem. These initial guesses do not incorporate obstacle-avoidance constraints. Four discomfort minimization problems, corresponding to these four initial guesses, are solved to find four trajectories. The lowest cost trajectory can be chosen as the final solution.

• Implement and solve. We use Ipopt, a robust large-scale nonlinear constrained optimization library (Wächter and Biegler, 2006) to solve the discretized problem.

## 4 Organization of this paper

This paper is organized as follows. Section 5 presents some preliminary material on the motion of nonholonomic mobile robot on a plane and on parametric curves. Section 6 lays out the mathematical foundation of our framework, and is followed by the numerical solution method in Section 7 and computing an initial guess in Section 8. Evaluation of the framework and results are presented in Section 9, followed by concluding remarks and direction for future work in Section 10.

## 5 Preliminary material

In this section, we present the notation and some preliminary material that is relevant to our formulation. We begin by an analysis of motion of a nonholonomic mobile robot moving on a plane. We then provide a brief introduction to parametric curves and arc-length parameterization of curves.

### 5.1 Motion of a nonholonomic mobile robot moving on a plane

The configuration of a rigid body moving on a plane at any time can be completely specified by specifying the position vector and orientation of a body-fixed frame with respect to a fixed reference frame. Suppose the rigid body starts from an initial configuration at time and reaches a final configuration at time . To fully specify the motion of the body it is necessary to specify the functions and on . If this body is a physical system, it cannot change its position instantaneously. Further, since forces of infinite magnitude cannot be applied in the real world, the acceleration of the body must be finite. Hence , and must be at least on .

If this rigid body has directional wheels, its motion should obey the following nonholonomic constraint

 ˙xsinθ−˙ycosθ=0. (1)

Here dot, , represents derivative with respect to . For motion planning, it is common to model a mobile robot as a wheeled rigid body subject to above nonholonomic constraint, and we will do the same. A motion of such a body can be specified by specifying a travel time and a trajectory for . The orientation can be computed from Equation (1). Essentially, . If is zero, which means the velocity is zero, then this equation cannot be used. If the instantaneous velocity is zero at , and non-zero in a neighborhood of , then can be defined as a .

### 5.2 Parametric curves and the arc-length parameterization

We present a brief introduction to parametric curves and the arc-length parameterization. The reader can refer to any book on differential geometry of curves for more details.

Let and . A planar parametric curve is a mapping . If components of are of class , the vector space of functions with continuous first derivatives, the tangent vector at for is . In this section, we denote derivatives with respect to the parameter by a prime .

Let the length of a curve be denoted by , where

 λ=∫qbqa∣∣∣∣r′(q)∣∣∣∣dq. (2)

Define a function , which is the length of the curve between . Then,

 s(q)=∫qqa∣∣∣∣r′(q)∣∣∣∣dq. (3)

Note that the integrand is non-negative throughout . We make an assumption that it is zero only at a finite number of ’s in . If represented time, the physical interpretation is that the velocity is equal to zero only at a finite number of discrete instants in time. This assumption implies that is an increasing function of . That is, if , then . This, in turn, means that for any given , a unique can be found that corresponds to that . If components of are of class , then is continuous, and thus is also in . Thus, is defined and is a continuous function. Obviously, .

With the assumption above that can be zero only at a finite number of ’s, it is possible to introduce the arc-length parameterization. For define

 ˆr(s)=r(q)wheres=s(q). (4)

The function is well-defined because for each a unique can be found. Using the chain-rule for differentiation,

 dˆrds=drdqdqds.

Now exists and is continuous and also exists (and is continuous) if is not zero. Thus, at points where ,

 ∣∣∣∣∣∣dˆrds∣∣∣∣∣∣=∣∣∣∣r′(q)∣∣∣∣/∣∣∣∣r′(q)∣∣∣∣=1.

On points where , cannot be computed by the expression above. However, the choice that makes it continuous for all is 1. This is analogous to computing the limiting value of the orientation when velocity is zero as shown earlier in this section.

Symbolically, the curve has been parameterized by the arc-length. Since , the tangent vector computed in the new parameterization is a unit vector. The tangent vector is and the unit normal vector is , where

 T(s)=dˆrdsN(s)=dTds∣∣∣∣dTds∣∣∣∣ (5)

See Figure 1. The signed curvature is defined as

 κ(s)=dθds (6)

where is the tangent angle.

## 6 Formulating motion planning as a constrained optimization problem

This section presents the mathematical formulation of our framework for planning comfortable and customizable motion of a planar nonholonomic mobile robot.

The steps involved are: (1) formulating a discomfort cost functional (Section 6.1) (2) dimensional analysis of cost functional (Section 6.2) (3) formulating an informal problem statement (Section 6.3) (4) choosing an appropriate parameterization of the trajectory (Section 6.4), (5) choosing the function space to which the trajectory should belong for the cost functional to be well-defined (Section 6.5), (6) analysis of boundary conditions to determine the boundary conditions that should be imposed for the problem to be well-posed (Section 6.6), (7) choosing a representation of obstacles and imposing constraints for obstacle avoidance (Section 6.7), and finally, (8) formulating the full infinite-dimensional constrained optimization problem (Section 6.8).

### 6.1 The discomfort cost functional

In Section 2.1, we saw that for motion comfort, it is necessary to have continuous and bounded acceleration along the tangential and normal directions. It is possible that the actual values of the bounds on the tangential and normal components are different. It is also desirable to keep jerk small and bounded. We model user discomfort as a weighted sum of the following three terms: total travel time, time integral of squared tangential jerk and time integral of squared normal jerk. Travel time is included because we make the justifiable assumption that a user would prefer to reach a goal as fast as is consistent with comfort. Thus, longer travel time implies greater discomfort. We will see later in Section 6.5 that this cost functional is mathematically meaningful only when both tangential and normal acceleration are continuous. Thus, we get continuous accelerations by construction. To keep accelerations within comfortable bounds, we impose explicit constraints on the maximum and minimum values.

We construct a cost functional as follows:

 J=τ + wT∫τ0(...r⋅T)2 dt + wN∫τ0(...r⋅N)2 dt. (7)

Here is the total travel time and is the position of robot at time . represents the jerk. and are the tangential and normal components of jerk respectively. We assume that is smooth enough for the cost functional to be well-defined. This means (at least) that the acceleration vector is continuous and normal and tangential components of jerk are square integrable.

The term is necessary. If it is not included in the functional, the optimal solution is to reach the destination at traveling at essentially zero speed in the limit (except perhaps at the end-points where the speed is already specified). Thus, minimizing just the integral terms will not lead to a good solution.

The weights ( and are non-negative known real numbers. We separate tangential and normal jerk to allow a choice of different weights ( and ).

The weights serve two purposes. First, they act as scaling factors for dimensionally different terms. Second, they determine the relative importance of the terms and provide a way to adjust the robot’s performance according to user preferences. For example, for a wheelchair, some users may not tolerate high jerk and prefer traveling slowly while others could tolerate relatively high jerks if they reach their destination quickly. The typical values of weights will be chosen using dimensional analysis.

### 6.2 Dimensional analysis of cost functional and determination of characteristic weights

Choosing the weights in an ad hoc manner does not provide weights that lead to similar comfort levels independent of the input (the boundary conditions). Moreover, since the different components of the total discomfort are different physical quantities, choice of weights should reflect this. In other words, for the total discomfort to make physical sense, the weights cannot be dimensionless numbers but should have physical units. We determine the weights using dimensional analysis (Langhaar, 1951). If the weights are chosen without the dimensional analysis step, the optimal trajectory will be different just by specifying the input in different physical units. In addition, using the same numerical weights for different tasks will not lead to similar quantitative discomfort level.

All the physical quantities in the cost functional (time, jerk) depend on only two units length and time . From Equation (7) we see that has dimensions due to the first term (). Thus should have dimensions . Similarly, the dimensions of is . Alternatively, since , and has dimensions .

We now determine the base values of weights analytically. The main idea behind determining the base values is that the correct base values should keep the maximum speed below the maximum allowable speed. A user can then customize the weights by multiplying the base values by a dimensionless constant that indicates user preference.

#### Weight for tangential and normal jerk

We first determine . Consider a one dimensional motion with a trajectory that starts from origin and travels a distance in an unknown time . The starting and ending speeds and accelerations are zero. We choose the exact form of the trajectory to be a quintic polynomial in time . This choice uniquely determines the trajectory. The reason we have chosen a quintic is that it minimizes integral of squared jerk (a third derivative), just like a cubic spline minimizes integral of squared second derivative. Additionally, we choose the quintic to satisfy the boundary conditions.

Let be the distance traveled in time . It is easily seen that the quintic

 s(t)=Lt3τ5(6t2−15tτ+10τ2)

satisfies all the boundary conditions. For such a trajectory, the discomfort functional is

 J=τ+wT∫τ0...s(t)2dt=τ+720L2wTτ5.

We do not know and yet. We first choose a that minimizes for all . This means

 τ=(3600L2wT)1/6.

Obviously, choosing a large value of will increase , which is natural because doing so penalizes jerk and would slow down the motion. We now choose a so that the maximum speed during the motion is , a dimensional velocity scale. It can be seen that the maximum speed occurs at and it is

 (2252048)1/3(L4wT)1/6.

Hence we choose

 wT=(2252048)2L4V6. (8)

The base value for the weight corresponding to the normal jerk () is chosen to be the same. We emphasize that both and will be present in a real problem and the maximum speed constraint is imposed explicitly rather than relying on weights. The analysis done here is to get dimensional dependencies of the base weight and reasonable proportionality constants using a simple problem that can be treated analytically. If a different problem is chosen, these base values will change.

#### Factoring the weights for customization

In the preceding discussion, we determined the base values of weights using simple analytical problems. We will refer to these base values as and . Let be the minimum turning radius of the robot. For any given input, we determine the characteristic length as max where is the straight line distance between the start and end points. The characteristic speed is the maximum allowable speed of the robot. The base values of weights are then computed as

 ˆwT=ˆwN=(2252048)2L4∗V6∗. (9)

The weights for the actual problem are chosen as a multiple of these base weights where the multiplying factors and are chosen by a user.

 wT=fTˆwT,wN=fNˆwN. (10)

### 6.3 Problem statement

We formulate motion planning as a constrained optimization problem as follows: Given the appropriate boundary conditions on position, orientation, and derivatives of position and orientation, bounds on curvature, speed, tangential and normal accelerations, the weight factors and (Equation (9)), and a representation of obstacles, find a trajectory that minimizes the cost functional representing user discomfort Equation (7) such that the trajectory satisfies boundary conditions, respects bounds, and avoids obstacles

We model the robot as a rigid body moving on a plane, subject to the nonholonomic constraint of Equation (1), and assume that the robot moves with non-zero speed except at a finite number of points. Let the robot start from at and reach in time (Figure 2). From the discussion in Section 5.1, we see that to fully specify the motion of the robot, we need only to specify a curve on ] such that the curve is at least continuous. Henceforth, in this chapter, we will use trajectory to refer to a function of robot position with respect to time.

We now transform the above problem description into a precise mathematical problem statement using a general nonlinear constrained optimization approach.

### 6.4 Parameterization of the trajectory

Mathematically, one can use different primary variables to describe a trajectory. For example, assuming the trajectory starts at zero time, one way to describe a trajectory is to provide the final time and the position vector as a function of time in between. Another way is to provide the final time and specify the orientation and velocity as functions of time. Another way is to represent the geometric path separately, using either position vector or orientation as a function of arc-length. The velocity at each point on the path is provided separately in this case.

We have found that making the assumption that speed be non-zero except at boundaries and expressing the trajectory solely in terms of speed and orientation as functions of a scaled arc-length parameter leads to relatively simple expressions for all the remaining physical quantities (such as accelerations and jerks). We shall see below, that with this parameterization, the primary variables (speed and orientation) and their derivatives enter the cost functional polynomially. This would not have been the case if everything were expressed in terms of as a function of time as we did in our previous work (Gulati et al., 2009).

In the following discussion, we implicitly assume that all the quantities have sufficient smoothness for expressions to be mathematically meaningful. In some cases, the derivatives appear not as point-wise values but inside an integral sign. In such a case we will assume that the integrands belong to an appropriate space of functions so that the integrals are well-defined. We explicitly state the requirements on the regularity when posing the optimization problem later in Section 6.5.

#### Scaled arc-length parameterization

Let . The trajectory is parameterized by . The starting point is given by and the ending point is given by . Let denote the position vector of the robot in the plane. Let be the speed. Both and are functions of . Let denote the length of the trajectory. Since only the start and end positions are known, cannot be specified in advance. It has to be an unknown that will be found by the optimization process.

Let be the arc-length parameter. We choose to be a scaled arc-length parameter where so that the unknown constant is not used in defining an unknown sized interval (as would be the case if was chosen as the arc-length parameter).

In the following discussion we will see that the trajectory, is completely specified by the trajectory length , the speed , and the orientation or the tangent angle to the curve. is a scalar while speed and orientation are functions of . These are the three unknowns, two functions and one scalar, that will be determined by the optimization process.

Since speed is the rate of change of arc length, we have

 v(u)=dsdt. (11)

Using in the above equation, we get

 dudt=v(u)λ. (12)

This gives,

 t=t(u)=∫u0λv(u)du. (13)

If is zero only at a finite number of points in , then is well defined for all .

Equation (13) is a key relation and gives us the means to convert between the time domain and scaled arc-length domain. We now introduce the third unknown – the orientation or the tangent angle to the curve . Using the results of Section 5.2, we can show that

 ∣∣∣∣r′(u)∣∣∣∣=λ. (14)

The tangent vector to the curve is given by

 r′(u)=∣∣∣∣r′(u)∣∣∣∣T(u)=λT(u) (15)

where is the tangent function.

 T(u)={cos(θ(u)),sin(θ(u))}. (16)

The braces enclose the components of a 2D vector.

Thus, can be computed via the following integrals.

 r(u)=r(0)+λ{∫u0cosθ(u)du,∫u0sinθ(u)du}. (17)

Now, if is known, can be computed from Equation (17). If and are known, can be computed from Equation (13). Using these two, we can determine the function .

We now have all the basic relations to use chain-rule to derive expressions for all the physical quantities needed to pose the constrained optimization problem. We drop explicit references to as a function parameter to keep the expression concise.

We compute first, second, and third derivatives of with respect to time. These expressions are easily derived in one or two steps of chain rule differentiation and so we do not present the intermediate steps in detail.

 ˙r = v{cosθ,sinθ} (18) ¨r = vλ(v′{cosθ,sinθ}+vθ′{−sinθ,cosθ}) (19) ...r = vλ2((v′2+vv′′−v2θ′2){cosθ,sinθ}) (20) + vλ2((3vv′θ′+v2θ′′){−sinθ,cosθ})

From the equations above, the expressions for tangential acceleration and normal acceleration are

 aT=¨r⋅T=vv′λ (21)
 aN=¨r⋅N=v2θ′λ. (22)

The tangential jerk is

 jT=...r⋅T=vλ2(v′2+vv′′−v2θ′2) (23)

and the normal jerk is

 jN=...r⋅N=v2λ2(3v′θ′+vθ′′). (24)

Here is the direction normal to the tangent (rotated anti-clockwise). The signed curvature is given by

 κ(u)=θ′λ. (25)

The angular speed is given by

 ω(u)=θ′vλ. (26)

We can use the Equations 23 and 24. to express the total discomfort

 J(r,τ)=∫τ0dt+wT∫τ0(...r⋅T)2dt+wN∫τ0(...r⋅N)2dt (27)

in terms of , , and . First, we express the travel time in terms of the primary unknowns.

 τ=∫dt=∫10dtdudu=∫10λvdu. (28)

Using a similar change of variables in the integration (), the total discomfort can be written as

 J(v,θ,λ)=∫10λvdu+wT∫10vλ3(v′2+vv′′−v2θ′2)2du+wN∫10v3λ3(3v′θ′+vθ′′)2du. (29)

The first integral () is the total time, the second integral () is total squared tangential jerk, and the third integral () is total squared normal jerk.

Note that except for the term due to total travel time, the primary variables and and their derivatives enter the total discomfort expression polynomially.

The discomfort is now a function of the primary unknown functions , , and a scalar , the trajectory length. All references to time have disappeared. However, once the unknowns are found via optimization, we must compute a mapping between and . This can be done using Equation (13).

### 6.5 Function spaces for v and θ for a finite discomfort

Now that we have a concrete expression for the discomfort in Equation (29), it can be used to define the function spaces to which and can belong so that the discomfort is well-defined (finite). This will, in turn, lead to conditions on the physical quantities for safe and comfortable motion. We have two distinct cases depending on whether the speed is zero at an end-point on not.

#### Conditions for positive speeds

Let and be the Sobolev space of functions on with square-integrable derivatives of up to order 2. Let . Then

 f∈H2(Ω)\lx@stackreldef⟺∫Ω(djfdxj)2dx<∞∀j=0,1,2. (30)

First, we show that if , then the integrals of squared tangential and normal jerk are finite. Using the Sobolev embedding theorem (Adams and Fournier, 2003) it can be shown that if , then and by extension . Here is the space of functions on whose up to derivatives are bounded and continuous. Thus, if , then all the lower derivatives are bounded and continuous. Physically this means that quantities like the speed, acceleration, and curvature are bounded and continuous all desirable properties for comfortable motion.

Expanding all the jerk related terms in Equation (29), bounding all the non-second derivative terms by a constant using the results from the Sobolev embedding theorem, we immediately see that the jerk part of discomfort is finite if . This is a sufficient condition only and not a necessary one as we shall see below.

We also need that the inverse of be integrable so that is finite. This is trivially true if is uniformly positive, that is, for some constant positive throughout the interval . However, can be zero at one or both end-points because of the imposed conditions. Section 6.6 analyzes the boundary conditions in detail. Here we assume that speed on both end-points is positive. The cases with zero end-point speed are treated below in Section 6.5.

Thus, consider the case that is positive on both end-points. Since is speed and always non-negative, it can approach zero from above only. We make a justifiable assumption that can be zero only at end-points if at all and not in the interior. Otherwise, the robot would stop and then start again. This is costly for discomfort since it increases travel time and leads to acceleration and deceleration. Of course, we can choose a motion in which in the interior and it can still be a valid motion with finite discomfort. The assumption is that the trajectory that actually minimizes discomfort will not have a halt in between. Thus, if on end-points, it remains uniformly positive in the interior and the discomfort is finite.

#### Conditions for zero speed on boundary

Consider the case in which . The case can be treated in a similar manner. If , must not blow up faster than where . This is to keep finite. This can be seen as follows. Lets assume for some (so that . This implies that provided , otherwise it is not defined.

For simplicity, assume a 1D motion so that . Then provided . Taking all conditions into account, if , the discomfort is finite if behaves like where . However, in such a case, is defined and finite only if . This conflicts with the assumption that . Thus, we can have a finite discomfort even if . We see that the reason for this is the zero speed boundary condition, which leads to being finite for even though (which is the highest order term in ) is not finite for such a range of .

If we look at the integral carefully, we see that it can be finite even if , provided . This is a special case because is identically zero for such a and tangential jerk discomfort is finite for a 1-D motion.

For a mathematically meaningful problem we must treat zero speed boundary conditions separately from non-zero speed boundary condition. This analysis will be done in more detail in Sections 6.6 and 7 which are focused on boundary conditions and appropriate singular finite elements respectively.

#### Summary

To summarize, the total discomfort is finite if and the inverse of is integrable. Inverse of is integrable if is uniformly positive in . If zero speed boundary conditions are imposed, we will have to choose outside . In such a case, at , it is sufficient that approaches zero as where or . For the right end point, where , replace with in the condition. We do not lose higher regularity of throughout the interval just because . Assume in the interior, as justified above. Then in where . Thus is necessary to keep total discomfort finite. This implies continuity and boundedness of velocity and acceleration in .

### 6.6 Analysis of boundary conditions

The expression for the cost functional in Equation (29) shows that the highest derivative order for and is two. Thus, for the boundary value problem to be well-posed we need two boundary conditions on and at each end-point one on the function and one on the first derivative.

We also have to impose that the robot move from a specific starting point to a specific ending point. This condition is a set of two equality constraints on and based on Equation (17). If the motion is from positions to , then

 rτ−r0=λ{∫10cosθdu,∫10sinθdu}. (31)

We now relate the mathematical requirement on and boundary values above to expressions of physical quantities. We do this for the starting point only. The ending point relations are analogous.

#### Positive speed on boundary

First, consider the case when on the starting point. The speed needs to be specified, which is quite natural. The -derivative of , however, is not tangential acceleration. The tangential acceleration is the -derivative and is given by Equation (21). It is . Here is known but is not. Thus specifying tangential acceleration gives us a constraint equation and not directly a value for . This is imposed as an equality constraint. Similarly, fixing a value for on starting point is natural. We “fix” the values of by fixing the signed curvature . As before, this leads to an equality constraint relating and if . Since choosing a meaningful non-zero value of is difficult, it is natural to impose . In this case can be imposed easily.

#### Zero speed on boundary

We now discuss the case. If , then, as seen in Section 6.5, must behave like for or near and for or respectively. This means the is infinite. This leads to a difficulty in analyzing the expression for the tangential acceleration () without using limits. We prove that if at boundary, then the tangential acceleration is 0 if and it is finite but non-zero if . If , then, . If , it means . Thus as , because of the allowable range of . If , behaves like a positive constant as . Hence corresponds to non-zero tangential acceleration.

We still have to decide with what strength does tend to infinity at an end-point. If and , it is clear that . If and , the analysis above has only shown that , and . In this case, we need to use the time domain. The reason we have such a singularity is because of working in the arc-length domain. Consider starting from origin with zero speed and acceleration at zero time () in 1D. Expanding the distance traveled () as a function of time, we see that

 s(t)=0+0t+120t2+16jt3+….

Here is the jerk at . We ignore the higher order terms. Then, to the lowest power of , the speed as a function of is

 v(t)=12jt2.

Eliminating to relate and , we get

 v=62/32j1/3s2/3.

Now because is the scaled arc-length parameter. Using this we get , where all the constants are absorbed in . Thus, for . This value of is within the acceptable range of , the open interval (). This also tells us that

 v′(u)∼u−1/3 (32)

is the appropriate strength of the singularity.

#### Summary

To summarize, we must specify following boundary conditions at both end points: position, orientation, curvature, and speed. If a specified speed is non-zero, the tangential acceleration must be specified. If the speed is zero, the tangential acceleration can be zero. If tangential acceleration is non-zero, it must be positive if it is the starting point or must be negative if it is the ending point.

### 6.7 Obstacle avoidance

For safe motion, it is necessary that the robot avoid obstacles while navigating. Simply speaking, obstacles are regions in the plane of motion through which the geometric path must not pass. Obstacles can be represented in a variety of ways. For example, convex polygons, rectangular cells, simple closed shapes like ellipses, or level sets of implicitly defined simple functions of two arguments are some possibilities.

#### Modeling obstacles as star-shaped domains

We have chosen to model the “forbidden” region formed by the obstacles as a union of star-shaped domains with boundaries that are closed curves with piecewise continuous second derivative. A set in is called a star-shaped domain if there exists at least one point in the set such that the line segment connecting and lies in the set for all in the set. Intuitively this means that there exists at least one point in the set from which all other points are “visible”. We will refer to such a point as a center of the star-shaped domain.

The choice of using star-shaped domains is made so that each point on the boundary of an obstacle can be treated as coming from a well-defined function in polar coordinates centered within the particular obstacle. See Figure 3. This also allows treatment of non-convex obstacles without subdividing them into a union of convex shapes. A big advantage is that we reduce the number of imposed constraints since the number of inequality constraints is proportional to the number of obstacles. This leads to a faster optimization process.

This approach is a special case of using level sets of an implicitly defined function as an obstacle boundary. What is different here is that given the description of the boundary in polar coordinates, which is easy to specify for common shapes, we construct an implicit function (see the following section). This is done based on the assumption that the boundary encloses a star-shaped region. The piecewise smoothness property is required to impose the obstacle constraint in a numerical optimization method. Since up to second derivative of constraint can be required, the obstacle boundary should also be smooth to that order (or at least piecewise smooth).

If an obstacle is not star-shaped, our framework can still handle it if it can be expressed as a finite union of piecewise smooth star-shaped domains. Efficient algorithms to decompose any polygon into a finite number of star-shaped polygons exist (Avis and Toussaint, 1981).

#### Incorporating constraints for obstacle avoidance

We now derive a function for the inequality constraint that a given point in the plane is not inside the boundary of one star-shaped obstacle. It is easy to extend this to multiple points and multiple obstacles by just repeating the inequality with different parameters.

Let an obstacle be specified by its boundary in polar coordinates that are centered at . Each gives a point on the boundary using the distance from the obstacle origin. The distance function must be periodic with a period . See Figure 3.

Suppose we want a point to be outside the obstacle boundary. Define as

 C(r)=||r−r0||2−ρ(arctan% 2(r−r0)) (33)

where the subscript 2 refers to the Euclidean norm. It is obvious that the point is outside the obstacle. This can be seen using a 1D graph of . For example, let an obstacle be represented as shown in Figure a. Figure b shows the same obstacle flattened out as a 1D curve. Then is positive in the top region and negative below. The star-shaped property leads to a single-valued curve when flattened like this. The vector is related to the primary variables in our optimization problem using Equation (17).

#### Derivatives of obstacle avoidance constraint

We will need derivatives of with respect to for incorporating as a constraint in the trajectory optimization problem. Here is any point on the path that we want to lie outside a given obstacle. We can derive the following expressions for first and second derivatives of . The derivatives of below are evaluated at . To simplify the expressions, refer to the offsets from obstacle origin instead of absolute positions in the plane.

 ∂C∂r=r||r||2−ρ′(ϕ){−y,x}||r||22 (34)
 ∂2C∂r2=1||r||32(1−ρ′′(ϕ)||r||2)[y2−xy−xyx2]−ρ′(ϕ)||r||42[2xyy2−x2y2−x2−2xy] (35)

Obviously, the second derivative is a matrix.

The constraint function is piecewise differentiable for all except at a single point . If by chance, which is easily detectable, we know that the is inside the obstacle and can perturbed to avoid this undefined behavior. Note that remains bounded inside the obstacle. It is the derivatives that are not bounded as Figure c shows a surface plot of for the obstacle shown in Figure a. The contours of constant values are shown in Figure d.

#### Incorporating robot shape

The discussion on obstacle avoidance constraints so far has assumed that the robot is a point. In reality, the robot is not a point. To impose obstacle avoidance constraints in this case, the robot can be modeled as a closed curve that encloses the projection of its boundary in the plane of motion. We can choose a set of points on this curve and impose the constraint that all these points be outside all obstacles. The distance between any pair of points can be smaller than the smallest obstacle. We have currently not implemented this and this is part of future work.

### 6.8 The full nonlinear constrained optimization problem

We now summarize the nonlinear and constrained trajectory optimization problem taking into account all input parameters, all the boundary conditions, and all the constraints. This is the “functional” form of the problem (posed in function spaces). We will present an appropriate discretization procedure valid for all input combinations in the next chapter.

Minimize the discomfort functional , where

 J(v,θ,λ)=∫10λvdu+wT∫10vλ3(v′2+vv′′−v2θ′2)2du+wN∫10v3λ3(3v′θ′+vθ′′)2du,

given the following boundary conditions for both starting point and ending point

• position (, ),

• orientation (, ),

• signed curvature (, ),

• speed (, ),

• tangential acceleration (, ),

and constraints on allowable range of

• speed (),

• tangential acceleration (),

• normal acceleration (),

• angular speed (),

• curvature, if necessary (),

and

• number of obstacles ,

• locations of obstacles

• representation of obstacles that allows computation of , for

and

• an initial guess for , in ,

• weights and .

The constraint on starting and ending position requires that

 rτ−r0=λ{∫10cosθdu,∫10sinθdu}

Staying outside all obstacles requires that

 ||r(u)−ci||2−ρi(arctan2(r(u)−ci))≥0∀i∈1,…,Nobs, and∀u∈[0,1]

where

 r(u)=r(0)+λ{∫u0cosθdu,∫u0sinθdu}.

As a post-processing step, we compute time as a function of using

 t=t(u)=∫u0λv(u)du

and convert all quantities ( and their derivatives) from domain to domain.

## 7 Numerical solution

The optimization problem posed in Section 6.8 is infinite dimensional since it is posed on infinite dimensional function spaces. This means that we must discretize it as a finite dimensional problem before it can be solved numerically. We saw in Section 6.6 that we must be able to impose two kinds of boundary conditions. In the first kind, the problem is set in the Sobolev space of functions whose up to second derivatives are square integrable. In the second kind, we must allow functions that are singular at the boundary (with a known strength) but still lie in the same Sobolev space in the interior. Keeping the problem setting and requirements mentioned above in mind, it is natural to use the Finite Element Method (FEM) to discretize it.

For the first kind of boundary conditions, where speed is non-zero at both boundaries, must belong to . It is natural to use the basis functions in , the space of functions that are continuous and have continuous first derivatives. In accordance with standard FEM practice (Hughes, 2000), we choose the cubic Hermite shape functions to discretize . For the second kind of boundary conditions, when either one or both the boundary points have zero speed specified, we must allow functions that are singular at the boundary (with a known strength) but still belong to in the interior. We use special shape functions for on the boundary element where speed is zero so that has the appropriate strength of singularity at the boundary, and use regular cubic Hermite shape functions in the interior. For both kinds of boundary conditions, we use cubic Hermite shape functions for on all elements.

With the above choice of shape functions, an element mesh for the problem consists of the following unknowns at each node – resulting in a total of unknowns. In addition, the path length is also an unknown. For optimization, the values of the objective function, its gradient and Hessian, the values of constraints, and the gradient and Hessian of each constraint are required. For efficiency, it is desirable that objective function and constraint Hessians be sparse. We show in the second paper of this series that the Hessian of obstacle avoidance constraints can be kept sparse if we introduce additional unknowns in the form of position at points on the mesh and a constraint between each pair of and . We choose uniformly separated points and let so that obstacle avoidance constraints are imposed at points in the interior of each element and at each of the nodes.

With the discretization above, the infinite dimensional problem of Section 6.8 is converted into a finite- dimensional nonlinear constrained optimization problem. The objective now is to determine the values of the unknowns that minimize the discomfort cost functional and satisfy the boundary conditions and constraints described in (Section 6.8). To impose boundary conditions, we impose constraints on some of the degrees of freedom and eliminate some of the degrees of freedom at the end points. To impose dynamic constraints, we compute the speed , tangential acceleration , normal acceleration , angular speed , and curvature in terms of the unknowns at points in the interior of each element and impose bounds on these quantities as constraints. See Figure 5.

To numerically compute the integrals in the cost functional and constraints, we use Gauss quadrature formulas with 12 integration points. In our implementation, the number of elements in the finite element mesh, , number of intervals per element for obstacle avoidance constraints, , the number of points per element on which to impose dynamic constraints . The rationale for choosing these particular values is discussed in the second paper in this series.

## 8 Initial guess for the optimization problem

Because of the non-linearity in the optimization problem, and the presence of both inequality and inequality constraints, it is crucial that a suitable initial guess of the trajectory be computed and provided to an optimization algorithm. Many software packages can generate their own “starting points”, but a good initial guess that is within the feasible region can easily reduce the computational effort (measured by number of function and derivative evaluation steps) many times. Not only that, reliably solving a nonlinear constrained optimization problem without a high quality initial guess can be extremely difficult. Because of these reasons, we invest considerable mathematical and computational effort to generate a high quality initial guess of the trajectory.

Our optimization problem is to find the scalar and the two functions and that minimize the discomfort. We compute the initial guess of trajectory by computing and first and then computing by solving a separate optimization problem. We emphasize that the initial guess computation process must deal with arbitrary inputs and reliably compute the initial guesses.

### 8.1 Initial guess of path

To compute initial guess for and for any pair of specified initial and final orientations , we solve an auxiliary (but simpler) nonlinear constrained optimization problem for the four pairs of orientations , , , and . We minimize

 J(θ,λ)=λ+w∫10θ′′2du (36)

where , and must satisfy the boundary conditions, the two equality constraints of Equation (31), and the curvature constraint We do not impose obstacle related constraints in this problem. Here is the distance between start and end positions and is the minimum allowed radius of curvature. Note that this is a geometric and time is absent.

This method of computing four different initial guesses is based on a special kind of non-uniqueness of paths. This particular kind of non-uniqueness arises because a single physical orientation can correspond to multiple numerical values of (). This is because is continuous and cannot jump to a different value in between. Of course, this optimization problem needs its own initial guess, and we use paths similar to Dubins curves to compute a suitable initial guess. Our method generates two different initial guesses for the auxiliary problem for the pair , leading to 4 initial guesses of for the discomfort minimization problem. is the length of the paths computed above. The non-uniqueness in paths and the computation of initial guess is discussed in greater detail in the second paper (Part II) of this series.

### 8.2 Initial guess of speed

For the case when both end-points have non-zero speed, we compute initial guess of by solving an auxiliary optimization problem. We minimize

 J(v)=∫10v′′2du (37)

subject to boundary constraints , , ,