Optimal motion planning based on path length minimisation

Accepted Manuscript Optimal motion planning based on path length minimisation Jinu Krishnan, U.P. Rajeev, J. Jayabalan, D.S. Sheela

PII: DOI: Reference:

S0921-8890(17)30025-8 http://dx.doi.org/10.1016/j.robot.2017.04.014 ROBOT 2834

To appear in:

Robotics and Autonomous Systems

Received date : 1 February 2017 Revised date : 14 April 2017 Accepted date : 24 April 2017 Please cite this article as: J. Krishnan, et al., Optimal motion planning based on path length minimisation, Robotics and Autonomous Systems (2017), http://dx.doi.org/10.1016/j.robot.2017.04.014 This is a PDF file of an unedited manuscript that has been accepted for publication. As a service to our customers we are providing this early version of the manuscript. The manuscript will undergo copyediting, typesetting, and review of the resulting proof before it is published in its final form. Please note that during the production process errors may be discovered which could affect the content, and all legal disclaimers that apply to the journal pertain.

Optimal motion planning based on path length minimisation Jinu Krishnan Vikram Sarabhai Space Centre, Indian Space Research Organisation

U. P Rajeev Vikram Sarabhai Space Centre, Indian Space Research Organisation

J. Jayabalan Vikram Sarabhai Space Centre, Indian Space Research Organisation

D. S. Sheela Vikram Sarabhai Space Centre, Indian Space Research Organisation

Abstract In this paper, the path planning problem for a single autonomous robot treading on a static terrain is solved using optimal control theory. The performance measure selected for the optimal control formulation is the path length between a predefined initial and final position on an inertial frame of reference. The terrain features are assumed to be known a priori and is represented using a b-spline surface. The terrain tractability constraint is decoupled from the dynamic stability equation of the robot and absorbed as a terrain feature. The results indicate that the target is reached optimally and the trajectory is smooth. The smoothness of the trajectory eliminates the need for post processing for trajectory tracking. This formulation can be applied to path planning problems related to aerial and ground vehicles with minimum modifications to take care of the real time requirements. Keywords: Optimal control, path planning, polar co-ordinate system, terrain representation, b-spline

Preprint submitted to Journal of Robotics and Automation

April 13, 2017

1. Introduction The earliest autonomous robot path planning algorithms were based on dynamic programming and graph theory. The pioneering work of Djkstra[1] and Bellman[2] on the problem of finding shortest path between any pair of nodes of an undirected graph was a cornerstone in the field of robot motion planning. In one of the oldest algorithms called the Roadmap method[3], the obstacles are assumed to be polygons and the vertices of these polygons form the nodes of a graph. Two nodes are connected with an edge if and only if the edge does not cross any obstacle[3]. The starting location and the target location also form nodes of the graph. The environment is also assumed to be static. The path planning problem is then solved as a shortest path finding problem on a directed graph. The disadvantage of this approach is the amount of computational time required for synthesising the graph and later solving the shortest path problem. Moreover, this approach fails in dynamic environments and nonflat terrains. Improvements to the above methodology were suggested in [4][5], wherein the terrain to be explored is divided into cells of fixed dimension. A tree is built by assimilating all free cells. The edge of the tree represents the path length between successive tree nodes. A search though the tree will yield the shortest path required. The disadvantage of this approach was that kinematic constraints(holonomic and non-holonomic) of the robot were not considered in the formulation. The use of analytical functions and optimization for path planning problem was first suggested by Krogh[6]. In this formulation, the author used the concept of potential to solve the path planning problem. The target location will generate an attractive potential field whereas obstacles generate repulsive potential fields. These potential fields are modelled as a smooth function of the position co-ordinate of the terrain. The local force acting on the robot at a specific instant on its trajectory is computed as the negative gradient of the total potential. But literature[3] shows certain terrain geographies on which the robot may get stuck. The objective in a path planning problem is to plan a path that min-

2

imises(maximises) a chosen performance measure honouring the static and dynamic constraints imposed by the environment. Optimal control theory is a natural candidate for solving problems of such genre. The performance index can be minimum time, minimum path length, minimum energy spend etc. The constraints can be static like terrain composition that makes motion intractable or dynamic like the limit on the speed of the robot to prevent slipping or toppling. An optimal control methodology based on minimum time criterion is described by Schiller et. al.[7]. The authors have considered a non-flat terrain that is modelled using b-spline surface patches. Their formulation yields a velocity profile that minimises travel time honouring kinematic constraints that prevent slipping or toppling of the robot on rough terrain. The authors also claim that a minimum time performance measure is superior to a minimum path length performance measure. The reason cited for this claim is that a minimum path between two points on the map may pass through terrain features that cannot be negotiated by the robot like sand, water, mud etc. unless these features are properly represented on the map. Minimising the total path length traversed is a natural and logical way of solving the path planning problem. For instance, a robot exploring an alien planet may have no advantage of traversing a time minimised trajectory. The map information of the alien planet itself would be uncertain, in which case a maximum velocity or minimum time profile may put the robot at a greater risk than following a shortest path length trajectory. The limitations cited by Schiller et.al.[7] on the minimum path length performance measure can be overcome using penalty methods. Intractable regions on the terrain can be considered as terrain formations with infinite elevations treading through which the robot will incur a penalty. Dellingete et.al.[8] and Reuter et.al.[9] describe a method based on trajectory curvature minimisation. In their work the authors claim that a path having large bend or curvature should be avoided because the robot’s control system may not be able to negotiate sharp turns in the trajectory satisfactorily that may compromise the integrity of the robot. Their methodology is based on defining a smooth trajectory with minimum curvature. For this requirement they have 3

considered clothoid functions having affine curvature. The authors try to fit a clothoid with minimum curvature between the start location and the target location. Frazzoli et.al.[10] considers a real time path planning problem on a non-flat terrain. The authors claim that their algorithm will cater to dynamic environments and rough terrains by using a hierarchical methodology consisting of three layers. The first layer plans an ideal trajectory, using the minimum time criterion, on a flat terrain devoid of any obstacles without considering the robot dynamics. In the second step, a 6D simulator is used to simulate the dynamics of the robot as it follows the ideal trajectory. A randomised path planner makes up the third layer that adjusts the ideal trajectory whenever the simulator detects a violation in the dynamic or static constraints or an obstacle is sensed. The purpose of this paper is to formulate the path planning problem as an optimal control problem with path length minimisation as the performance measure. The path planning problem is first formulated in a right handed cartesian coordinate system. Simulations show that for certain type of terrain geography, this formulation generates infeasible trajectory. This deficiency is overcome by reformulating the algorithm in polar co-ordinate system without altering or augmenting the problem statement and constraints. The novel contributions brought out in this paper are the following. • The basic problem cited in literature[7] that discourages a minimal path length planning is the terrain tractability. There may be forbidden regions like mud, water etc.. through which a robot cannot move and these regions act as additional constraints on the optimal control problem. The minimal path length formulation solution may force the robot to move along these regions jeopardising the mission. The tractability of a region is described by its mobility coefficient[7] µm ∈ (0, 1) with zero being most mobile or tractable region and one being non-tractable region. The usual practice of handling tractability constraints is to absorb it as a part of the robot’s dynamic stability equations[7]. Deviating from this usual practise, a novel proposal of considering the tractability constraint as a terrain fea-

4

ture is described in this work. It is shown that if the terrain is modelled using a smooth mathematical structure like b-spline, then these forbidden regions can be considered as regions having elevations proportional to its mobility level. This means that regions through which the robot cannot move, will get very large elevation values and regions that is slower for the robot to navigate will have an elevation value greater than a smoother terrain surface. Thus the mobility coefficient(µm ) is absorbed in the performance index as part of the terrain geography as elevation information proportional to µm . Since, the optimal control problem considers the 3 dimensional path length as the performance index, the terrain regions with high elevation(least tractable regions, µm ∼ 1.0) will be automatically avoided due to the incurrence of penalty in path length. • A new formulation to the path length minimisation problem defined in polar co-ordinate system is presented in this work. The rectangular cartesian co-ordinate system forms the basis of common mathematical formulations due to the ease of visualisation and implementation. The limitations in the rectangular co-ordinate system in the representation of path length along a curved surface is brought out in this work. Even though this limitations can be avoided by explicitly defining constraints on the state variables, the solution of the optimal control problem becomes complicated by these additional constraints. However, it will be shown that in polar co-ordinate system these constraints are implicit, reducing the complexity of the solution process. This approach also avoids the use of post smoothing techniques for better negotiation by the robot. The paper is organised as follows. In section 2 the terrain representation aspect using b-spline patch is described. In section 3, the formulation of the path planning problem as a minimum path length optimal control problem is detailed. This formulation is devised on a cartesian co-ordinate system. The limitations encountered in the cartesian formulation resulting in infeasible trajectory for certain terrain formations is also explained in this section. Section 4 5

proposes a modified formulation in polar co-ordinate system that circumvents the infeasible trajectory generation. Section 5 discusses the results from the modified formulation on various test cases establishing the robustness of the formulation.

2. Terrain Representation The most important input to the path planning problem is the terrain model. Efficient representation of the terrain capturing the essential details and ensuring mathematical tractability is a challenge. Mathematical tractability refers to a representation of the terrain as a continuous and differentiable function. The very first algorithms used simple polygonal shapes in 2D to represent obstacles. In these representations, obstacles are terrain formations that cannot be negotiated by the robot. Such type of representation is valid only in flat terrains like the interior of a building and fails in outdoor environments where the terrain is non-flat. A non-flat terrain is negotiable for the robot if it can go over formations that will otherwise be deemed obstacles on a flat terrain. Elfes et.al.[4] in their work used occupancy matrix to model the terrain. In this approach, the terrain is divided into a matrix T of size M × N . Each entry Ti,j in the matrix is termed as a cell and carries a value called occupancy value that corresponds to the elevation of the terrain at the point (i, j) from a known reference point. Gaw et.al[11] describe a terrain representation using contour or relief maps. The terrain is projected onto a 2D space termed as a 2 21 D space. The contour lines are then approximated using a polynomial fit and a path is planned using optimal control methodology. In Computer Aided Modelling(CAM), spline curves are used to represent complicated geometric shapes. The extension to spline curves to model terrain features and formations was explored by Schiller et.al.[7] The polynomial properties of the splines ensure that the surface generated is a smooth function. Hence the same approach is adapted in this paper. A b-spline patch is a surface primitive made of cubic b-spline curves with the following parametric equation.

6

(a)

(b)

Vi ew

ew Vi

B

Yw

A

Zw View C

ew Vi

View C

Vi ew

A

B

Xw

View A

(c)

(d)

Zw

Zw

View B

View C

Figure 1: (a)The contour plot of a terrain, Zw axis is normal to the plane of the paper positive towards the reader (b)Section view from plane A (c)Section view from plane B (d) Section view from C

p(u, v) = uM RM T v T

(1)

    where, u = u3 , u2 , u, 1 , u ∈ [0, 1] and v = v 3 , v 2 , v, 1 , v ∈ [0, 1] are the parameter variables. u and v are the known independent parameter variable denoting a location(X and Y co-ordinate) on the terrain map scaled to the interval [0, 1]. The function p(u, v) is the elevation computed from the b-spline surface at the location denoted by the parameterised vectors u and v . In this paper, bold notations denote vector, capital letters denote matrices and normal letters represent scalars. The matrix M ∈ <4 × <4 that is a constant matrix

7

defines the type of spline, for cubic spline  −1   3 M =  −3  1

3

−3

−6

3

0

3

4

1

 1   0   0  0

(2)

The matrix R ∈ <4 × <4 defines the set of control points that has to be estimated from the sampled elevation information available about the terrain. The control points define the shape of the spline. Hence their selection is crucial to faithfully replicate the terrain features. If the terrain elevation information with respect to a chosen reference datum is available, the control points for the spline surface can be derived using a least square fit on the elevation information. Equation 2 defines a b-spline patch made up of 16 control points that represents only a portion of the entire terrain. Inorder to cover the entire stretch of terrain many such b-spline patches are sewn together at the junctions in a smooth fashion[12]. Figure 1(a) shows a terrain in contour representation. A least square b-spline surface is constructed using the elevation information. The subplots 1(b-d) show the comparison of the terrain elevation information between the actual terrain and the b-spline surface fit terrain for 3 different section views as illustrated in 1(a). A common inference that can be drawn from the section views 1(b-d) is that the b-spline surface captures the trend(slope) of the surface faithfully as a smooth function. Even though, it is observed that in 1(c), the undulating terrain with sharp corners(during the initial portion) is not exactly reproduced in the b-spline surface representation, the slope information is captured. The objective of the path planning algorithm is to construct a trajectory based on the terrain geography that undergoes minimal changes during the tracking phase. Hence the requirement of the algorithm is to have the terrain information as a smooth function preserving the slope information to plan the trajectory. The penalty incurred during the generation of a smooth function is the averaging of sharp non-continuous features on the terrain. But since the terrain slope 8

information is preserved, the algorithm is not losing any required information. Moreover, during the trajectory tracking phase the robot will be sensing the terrain in real time and correcting its course based on the terrain geography. This illustrative example is synthesised in python using the scipy library that has built in functions for b-spline surface representation using a least square error fit.

3. Path planning algorithm formulation The co-ordinate system used for the algorithm formulation is a right handed cartesian co-ordinate system as illustrated in figure 2. The co-ordinate system is inertial meaning that it is a non-accelerating frame fixed in space and frozen at the time when the robot starts the exploration task.

Zw

Zw Robot

Yw

u Xw

Yw

Xw Figure 2: Definition of the world co-ordinate system. The definition of the control variable u as the commanded direction of the robot measured positive from Xw axis on the world frame is also shown

9

The term inertial frame is synonymous to the world frame[3] used in most of the literature pertaining to robotics. Xw , Yw and Zw denotes the axes of the world frame on which the formulation is developed. Yw points towards the local north and Zw is orthogonal to Yw and is normal to the terrain surface. Xw is then unambiguously defined as orthogonal to the plane containing Yw and Zw completing the right handed system. In this formulation, it is assumed that the terrain features are known apriori in the world frame. The system model of the robot is described as follows. The robot moves with a constant velocity v at a predefined heading angle u. The heading angle u is measured positive anticlockwise from Xw axis. This is illustrated in figure 2. In this formulation, the heading angle u is considered as the control variable. The states of the system are the x and y co-ordinates of the robot in the world co-ordinate system. The state equations are the following

x˙ = v cos u

(3)

y˙ = v sin u

(4)

The states of the system are the x and y co-ordinates of the robot in the world co-ordinate frame. The dynamic constraints of the terrain like friction and traction can be accommodated as a state equation on the velocity v of the robot. The focus of this work is to derive a formulation for an optimal trajectory using path length as a performance index considering the geography of the terrain. Let f (x, y) be a smooth surface representative of the true terrain encountered by the robot. Let (xs , ys ) be the initial position of the robot in the world frame and let (xf , yf ) be the target position in the world frame. Figure 3 illustrates a path on the world co-ordinate frame. The performance index to minimise is the total path length.

10

Zw

dz`

O

ds

dx

dy Yw

Xw

Figure 3: Illustration of performance index

Let ds be an infinitesimally small portion of the path as shown in figure 3. Since ds is infinitesimally small, it can be considered as a straight line. Let dx, dy and dz be the changes along the Xw , Yw and Zw axis for a path length of ds, then

p (dx)2 + (dy)2 + (dz)2

ds =

(5)

The performance measure is then derived as the summation of ds throughout the path J=

Z

xf

xs

J=

Z

xf

xs

Z

yf

ys

Z

yf

ys

s

Z

zf

zs

p (dx)2 + (dy)2 + (dz)2

(dx)2 + (dy)2 +



∂f ∂f dx + dy ∂x ∂y

(6) 2

(7)

The optimisation problem is to determine the state y as a function of x when x is chosen as the independent variable. Combining equations 3 and 4 to

11

eliminate dy yields the modified state equation as given below dy = tan u dx

(8)

Equation 7 can be rewritten as s  2  2 Z xf dy ∂f ∂f dy 1+ J= + dx + dx ∂x ∂y dx xs Let f1 (u) =

dy dx ,

J=

(9)

substituting equation 8 into equation 9 yields the following Z

xf

xs

s


1 + tan2 u +



2 ∂f ∂f + tan u dx ∂x ∂y

(10)

Any value assumed by the control variable u can be represented in the interval [0, 2π]. Hence the control variable can be treated as unbounded simplifying the optimal control algorithm. The optimisation problem is formally stated[13][14][15][16] as follows: Given a starting location of the robot as (xs , ys ) ∈ <2 and a target loca-

tion as (xf , yf ) ∈ <2 minimise the path length functional J : f (
dy dx

= tan u is satisfied. The optimisation

variables are the state vector y ∈
minimise

y∈
subject to

Z

xf

xs

s


1 + tan u + 2



∂f ∂f + tan u ∂x ∂y

dy(i) = tan u(i), 2 ≤ i ≤ N − 1 dx x(1) = xs x(N ) = xf y(1) = ys y(N ) = yf

N is the number of discretisation interval or steps

12

2

dx

The Hamiltonian is derived as s


1 + tan u +

H(y, u, λ) =

2



∂f ∂f + tan u ∂x ∂y

2

+ λ tan u

(11)

where λ(x) is the lagrangian multiplier. Applying the optimality condition ∂H ∂u

"

= 0, gives the quadratic function representing the control u∗ (x) as

∂f 1+ ∂y

2

−λ

2

"

1+



∂f ∂y

2 ##

  ∂f ∂f ∂f ∂f ∂f + − λ2 ∂x ∂x ∂y ∂y ∂x "  2  2 # ∂f ∂f + − λ2 1 + =0 ∂x ∂x

(tan2 u) + 2 tan u



(12) Since equation 12 is quadratic in u, a root selection scheme should be followed for u. A positive root represents a positive(anticlockwise) heading angle and a negative root represents a negative(clockwise) heading angle. The choice of the root should be based upon the orientation of the final position with respect to the initial position of the robot. The specifics of the root selection scheme will be elaborated in section 5. The co-state equation is derived as    2 ∂f ∂f tan u ∂∂yf2 + ∂x∂y − ∂f ∂x + ∂y tan u dλ = r 2 dx
 ∂f 1 + tan2 u + ∂f ∂x + ∂y tan u

Let f2 (x, u, λ(x)) =

dλ dx ,

(13)

the differential equations 8 and 13 should be solved

along with the boundary conditions (xs , ys ) and (xf , yf ) to obtain the optimal heading u∗ (x) for the robot. This is a non-linear two-point coupled boundary value problem. Hence an analytical solution is formidable. The two-point boundary value problem is solved using the method of variation of extremals detailed in Kirk et. al.[17]. Since the terrain is modelled as a b-spline surface made up of cubic splines, the partial derivatives are easy to estimate. This numerical algorithm is illustrated in 5. The basic idea of the algorithm is to guess the initial value of the co-state variable λ(xs ) and then integrate the state and 13

co-state equations from xs to xf . If the final state y(xf ) after the integration does not match the boundary condition y ∗ (xf ) then the initial guess λ(xs ) is modified in a direction to bring y(xf ) closer to y ∗ (xf ).

Local minimum

Good initial guess for Newton method

⇤

(xs )

Infeasible region

(xs ) Figure 4: Sensitivity of λ(xs ) on the final value y(xf )

The challenge associated with this type of approach, as pointed out by Bryson et.al[18], is that the procedure will converge to an optimum only if the initial guess λ(xs ) is close to the optimum value of λ∗ (xs ). Figure 4 shows a graph of y ∗ (xf ) − y(xf ) for different guess of λ(xs ). A poor guess of λ(xs ) will force the solution to hit a local minimum as shown in figure 4 for λ(xs ) = 0.01, whereas a intelligent guess of λ(xs ) = 0.04 will facilitate a global minimum. Hence a good algorithm should first figure out the best possible λ(xs ) so as to ensure a global minimum. Figure 5 depicts a flow chart summarising the algorithm. The solution procedure can be divided into two major parts. In the first part a line search is employed to compute a good initial guess λ(xs )

14

that proceeds as follows. λ(xs ) is first initialised to a user chosen value γ. The state and co-state equations are then forward integrated to arrive at y(xf ). If |y ∗ (xf ) − y(xf )| < ζ then the solution proceeds to the second part. Otherwise

the initial guess λ(xs ) is iterated with a step size of ∆. If |y ∗ (xf ) − y(xf )| > ζ

even after M iterations then the process is repeated with a new random guess γ. The first part of the solution process callec the Outer Loop is similar to the bracketing methodology adopted in function optimisation paradigm[19], wherein an interval is computed in which the objective function is unimodal. Another important inference that can be drawn from figure 4 is that the function y ∗ (xf )− y(xf ) is undefined for λ(xs ) > 0.048 because the discriminant of equation 12 becomes negative. This observation further prompts the use of an iterative scheme for finding a feasible region for the co-state varibale λ(x). In the second part of the solution process called the Inner Loop, the initial co-state λ(xs ) from the former step is used to seed the newton-raphson method detailed in figure 5. Newton-Raphson method has quadratic convergence in the neighbourhood of the root and hence used to obtain the root of the function |y ∗ (xf ) − y(xf )|. But in certain cases wherein the initial guess is far away from the root location, the newton-raphson method may oscillate or stagnate. This issue is solved by bounding the number of iteration steps to N , beyond which the initial guess λ(xs ) is revisited by executing the former step as shown in figure 5. Each of the various blocks numbered 1 − 12 constituting the algorithm in figure 5 is explained in tables 1 and 2

15

16

Start

> (xs )

4

x=x+

x 6= xf

x

10A

i 1 xs )

Stop

✏

No

f(

0, x

= (xs ), i

= y ⇤ (xf )

i 1 xs ) ⌘ df ( ixs 1 ) i 1 d xs

6

No

i>N

7

Yes

x=x+

x

x) = (x) + f2 (x, u, (x))

x) = y(x) + f1 (u)

u = g(x, (x))

No

j =j+1

D⇣

xs

y(x +

i xs , x

12

6A

(xs ) = (xs ) +

Integration step-Block I

8

7A Maximum iterations reached

(x +

(xs ) =

x = xf

xs

Figure 5: Cartesian system: Algorithm steps

y(xj)

9

1, x

y(xf )|

x = xf

D = |y ⇤ (xf )

f( ⇣

i 1 xs )

x) = y(x) + f1 (u)

u = g(x, (x))

No

j>M

3

xs , (xs ) =

x) = (x) + f2 (x, u, (x))

y(x +

2

Inner Loop

11

=

i 1 xs

0 xs

5

(x +

Step 2(Newton-Raphson)-Block B

Yes

f(

i xs

Step 1(Bracketing)-Block A

Outer Loop

10

Yes

j

Integration step-Block I

Select another random

3A

1

x 6= xf

Yes

Step ID

Functionality

1

Algorithm Start. Guess a γ value(γ = 0.0 is the default). γ is a user defined input parameter to the algorithm

2

Initialize outer loop variables. Loop variable j = 1, independent variable x = xs and initial co-state guess λ(xs ) = γ

3

Decision box: Check if outer loop has exceeded the iteration limit M . M is user defined input parameter to the algorithm

3A

Decision box 3 is YES: If outer loop iteration exceeds limit M , then choose a new γ value as γ ∼ U (0, 1) ∗ δ. The algorithm is then restarted from step 1. δ is user defined input parameter to the algorithm that represents the maximum value allowed for the costate λ(xs )

4

Integration step: Compute control function u at (x + ∆x) by solving equation 12 and integrate state and co-state equations 8 and 13 from start position xs to final position xf . After this step the state vector y and control vector u are obtained

5

Compute the error in final state as D and initialise variables λ0xs which is the initial guess for the Newton-Raphson method and inner loop variable i for the Newton-Raphson method

6

Decision box: Check if D is less than threshold ζ. ζ is user defined input parameter to the algorithm

6A

Decision box 6 is NO: If the extremal variation D is beyond the threshold, the outer loop executes with a new starting value of λ(xs ) incremented by step size ∆. ∆ is user defined input parameter to the algorithm that denotes the step size of the bracketing scheme.

7

Decision box 6 is YES: Decision box: Check if the inner loop iterations has exceeded the limit N . N is user defined input to the algorithm.

Table 1: Cartesian Algorithm: Outer loop step description

17

Step ID

Functionality

7A

Decision box 7 is YES: This implies that the Newton-Raphson method has not converged, hence the outer loop executes with a new starting value of λ(xs ) as decribed in step 6A

8

Decision box 7 is NO: Integration step: This is same as step 4. Compute control function u at (x + ∆x) by solving equation 12 and integrate state and co-state equations 8 and 13 from start position xs to final position xf . After this step the state vector y and control vector u are obtained

9

Compute the error at the extremal for the current iteration i as f (λ(xi−1 s ))

10

Decision box: Check if the variation of the extremal computed in step 9 is less than threshold .  is user defined input parameter to the algorithm.

10A

Decision box 10 is YES: Algorithm converged and u∗ is the optimal control vector and y ∗ is the optimal state vector as derived in the integration step 8

11

Decision box 10 is NO: Update the co-state variable as per Newton-Raphson method so that the variation of the extremal is reduced.

12

Run the inner loop(Newton-Raphson method) with the updated initial co-state λ(xs ) and proceed to step 7

Table 2: Cartesian Algorithm: Inner loop step description

Figure 5 highlights the two major steps involved in the solution process. Block A represents the bracketing schem that is used to compute a viable initial condition for the subsequent Newton-Raphson method. Block B represents the Newton-Raphson method iterations.

18

Yw

3.1. Results and Discussion

Xw

Figure 6: Optimal path for a paraboloid surface

Figure 6 shows the contour map of a paraboloid surface f (x, y) = x2 + y 2 . It also shows the optimal path generated by the algorithm, for a robot from an initial point (0, 0) to target point (20, 20). The step size selected for the independent variable x is 1 unit and the variation of extremal method is used for solution. The optimal trajectory dictates the robot to jump 24 units along the y axis for a unit increase in the x co-ordinate. This is infeasible because the robot has to jump the valley and proceed along the contour of the paraboloid. The optimal trajectory should have a heading angle of 45o as shown in figure 6.Figure 7(a) shows the 3D plot of the scenario and figure 7(b) illustrates the scenario depicted on the Yw Zw plane. D1 is the length seen by the algorithm when choosing y = 24 for x = 1, but the actual length between the points is D2 >> D1.

19

(a)

(b)

Zw

D

1

Zw

D

2

Yw

Xw

Xw

Figure 7: Illustration of the path length chosen

The algorithm is not taking into account the curvature of the surface while determining the path length between two chosen points. Instead it tries to find piecewise line segments unrestricted along Yw axis so as to minimise the cumulative path length. Had there been a provision in the formulation to determine D2, it would not have chosen this infeasible path. An additional constraint like x2 + y 2 = R2 can be added to the formulation to restrict the incremental magnitude change in state y. This calls for an additional co-state variable addition to the formulation. The same constraint can be imposed implicitly if the algorithm is reformulated in the polar co-ordinate system (r, θ) with r being the independent variable and θ the state variable. Since r is the independent variable, its variation will be fixed. Hence irrespective of the value of the state variable θ the maximum variation in the position as seen in an equivalent cartesian co-ordinate system will be bounded by r. This reformulation of the algorithm in polar co-ordinate system is detailed in the next section.

20

4. Reformulation in Polar co-ordinate system In the previous formulation in cartesian co-ordinate system, the terrain elevation z was a function of x and y. The corresponding transformation in polar system is the following

x = r cos θ

(14)

y = r sin θ

(15)

z = f (r, θ)

(16)

The derivatives dx and dy becomes dx = cos θdr − r sin θdθ

(17)

dy = sin θdr + r cos θdθ

(18)

dz =

∂f ∂f dr + dθ ∂r ∂r

(19)

Since the state equation 8 holds, sin θdr + r cos θdθ = tan u cos θdr − r sin θdθ Further factorisation yields the following state equation dθ 1 = tan(u − θ) dr r

(20)

Before dwelling into the specifics of the formulation, the difference between the control variable u(r) and θ(r) should be understood. The control variable u(r) is the heading angle required to meet the target condition, whereas θ(r) is the current azimuth of the robot in the polar co-ordinate system. Both u(r)

21

and θ(r) are positve in the anticlockwise sense. The performance index that is derived as the following

J=

Z

rf

rs

Let f1 (u) =

J=

Z

rf

rs

dθ dr ,

Z

θf

θs

s

(dr)2 + r2 (dθ)2 +



∂f ∂f dr + dθ ∂r ∂θ

2

(21)

Substituting equation20 into equation 21 gives

v " u  2 #  2 u ∂f ∂f ∂f 2 ttan2 (u − θ) 1 + 1 ∂f tan(u − θ) + 1 + dr + r2 ∂θ r ∂θ ∂r ∂r (22)

The optimisation problem[13][14][15][16] can be stated as Given a starting location of the robot as (rs , θs ) ∈ <2 and a target loca-

tion as (rf , θf ) ∈ <2 minimise the path length functional J : f (
dθ dr

=

1 r

tan(u − θ) is satisfied. The opti-

misation variables are the state vector θ ∈
θ∈
subject to

J dθ(i) 1 = tan(u(i) − θ(i)), 2 ≤ i ≤ N − 1 dr r r(1) = rs r(N ) = rf θ(1) = θs θ(N ) = θf

N is the number of discretisation interval or steps

22

The hamiltonian is the following v " u  2 #  2 u 1 ∂f 2 ∂f ∂f ∂f 2 t + tan(u − θ) H(u, θ, λ) = tan (u − θ)) 1 + 2 +1+ r ∂θ r ∂θ ∂r ∂r

λ + tan(u − θ) r (23)

As discussed in the former section, the control u(r) is unbounded, hence using the necessary condition for optimality

∂H ∂u

= 0 provides the quadratic

function of control u(r) as 

  2 !  2 λ 1 ∂f  − 1+ 2 r r ∂θ #  2  2 ! λ 1 ∂f 1 ∂f ∂f 1 ∂f ∂f − 1+ 2 r ∂θ r ∂θ ∂r r r ∂θ ∂r  2  2 "  2 # 1 ∂f ∂f λ ∂f + 2 − 1+ =0 r ∂r ∂θ r ∂r

1 tan2 (u − θ)  1 + 2 r " +2 tan(u − θ)



∂f ∂θ

2 !2

(24)

The co-state equation is derived as   2 tan(u−θ) ∂f ∂ 2 f tan(u−θ) ∂ 2 f ∂f ∂f ∂ 2 f ∂f ∂f − tan(u−θ) + + + 2 2 r ∂θ ∂θ r ∂r ∂θ r ∂θ∂r ∂θ ∂r ∂r∂θ dλ s =    2  2 dr ∂f ∂f tan2 (u − θ) 1 + r12 ∂f + 2r tan(u − θ) ∂f ∂θ ∂θ ∂r + 1 + ∂r

(25)

Let f2 (r, u, λ(r)) =

dλ dr ,

due to the non-linear nature of the state and co-

state equation, analytical solution is not possible. The method of variation of extremals procedure as explained in the section 3 can be used to solve the state and co-state equation for the optimal control u∗ (r). The solution flow is same as depicted in figure 5 except translation of the state variables from cartesian to their polar counterparts. The solution flow graph of the algorithm is reproduced in figure 8 for better clarity and understanding. The steps in the algorithm are also explained, as done in cartesian case, in the tables 3 and 4

23

Step ID

Functionality

1

Algorithm Start. Guess a γ value(γ = 0.0 is the default). γ is the user defined input parameter to the algorithm.

2

Initialize outer loop variables. Loop variable j = 1, independent variable r = rs and the initial co-state guess λ(rs ) = γ

3

Decision box: Check if outer loop has exceeded the iteration limit M . M is user defined input parameter to the algorithm

3A

Decision box 3 is YES: If outer loop iteration exceeds limit M , then choose a new γ value as γ ∼ U (0, 1) ∗ δ. The algorithm is then restarted from step 1. δ is user defined input parameter to the algorithm that represents the maximum value allowed for the costate λ(rs )

4

Integration step: Compute control function u at (r + ∆r) by solving equation 24 and integrate state and co-state equations 20 and 25 from start position rs to final position rf . After this step the state vector θ and control vector u are obtained.

5

Compute the error in final state as D and initialise variables λ0rs and inner loop variable i for the Newton-Raphson method

6

Decision box: Check if D is less than threshold ζ. ζ is user defined input parameter to the algorithm

6A

Decision box 6 is NO: If the extremal variation D is beyond the threshold, the outer loop executes with a new starting value of λ(rs ) incremented by step size ∆. ∆ is user defined input parameter to the algorithm that denotes the step size of the bracketing scheme.

7

Decision box 6 is YES: Decision box: Check if the inner loop iterations has exceeded the limit N . N is user defined input parameter to the algorithm.

Table 3: Polar Algorithm: Outer loop step description

24

25

Start

> (rs )

r 6= rf

r

10A

Stop

✏

No

f(

=

i 1 rs )

i 1 rs

0, r

r) = ✓(r) + f1 (u)

u = g(r, (r))

No

j>M

3

rs , (rs ) =

= (rs ), i i 1 rs ) df ( irs 1 ) d irs 1

= |✓⇤ (rf )

f(

6

No

i>N

7

Yes

r=r+

r

r) = (r) + f2 (r, u, (r))

r) = ✓(r) + f1 (u)

u = g(r, (r))

No

j =j+1

D⇣

rs

✓(r +

i rs , r

12

6A

(rs ) = (rs ) +

Integration step-Block I

8

7A Maximum iterations reached

(r +

(rs ) =

r = rf

Figure 8: Polar formulation: Numerical algorithm

✓(rf )|

9

rs

✓(rf )|

1, r

D = |✓⇤ (rf )

r = rf

r) = (r) + f2 (r, u, (r))

✓(r +

2

Inner Loop

11

i rs

0 rs

5

(r +

Step 2(Newton-Raphson)-Block B

Yes

f(

i 1 rs )

10

Step 1(Bracketing)-Block A

Outer Loop

4

r=r+

j

Yes

Integration step-Block I

Select another random

3A

1

r 6= rf

Yes

Step ID

Functionality

7A

Decision box 7 is YES: This implies that the Newton-Raphson method has not converged, hence the outer loop executes with a new starting value of λ(rs ) as described in step 6A

8

Decision box 7 is NO: Integration step: This is same as step 4. Compute control function u at (r + ∆r) by solving equation 24 and integrate state and co-state equations 20 and 25 from start position rs to final position rf . After this step the state vector θ and control vector u are obtained.

9

Compute the error at the extremal for the current iteration i as f (λ(rsi−1 ))

10

Decision box: Check if the variation of the extremal computed in step 9 is less than threshold .  is user defined input parameter to the algorithm.

10A

Decision box 10 is YES: Algorithm converged and u∗ is the optimal control vector and θ ∗ is the optimal state vector as derived in the integration step 8

11

Decision box 10 is NO: Update the co-state variable as per Newton-Raphson method so that the variation of the extremal is reduced.

12

Run the inner loop(Newton-Raphson method) with the updated initial co-state λ(rs ) and proceed to step 7

Table 4: Polar Algorithm: Inner loop step description

5. Results and Discussion 5.1. Selection of the user defined algorithm parameters In the algorithms presented in section 3 and section 4 several user defined input parameters are mentioned. Some of these parameters are sensitive to the convergence rate of the algorithm. Since the simulation results presented 26

are for the polar algorithm, the heuristic for the selection of these user defined parameters for the simulation test cases are described in this section. A similar heuristic approach can be applied for the cartesian algorithm.

(a)

(b)

Figure 9: (a)The terrain used for analysing the behaviour of the optimal costate value at start(rs ) (b)The path finding algorithm was executed with 11 different starting locations at the same orientation(θ(rs ) = 0.0) from (1, 0o ) to (11, 0o ) with the same target location (33, 180o ). The graph shows the variation of the optimal costate with change in start location.

Initial guess of the costate variable at the start location γ : The inner loop of the algorithm uses the Newton-Raphson method that has quadratic convergence in the neighbourhood of optimality. Hence a good initial guess of the costate variable λ(xs ) will help in drastically reducing the number of iterations to achieve optimality. Eventhough the neighbourhood of the optimal solution depends on the terrain shape and may be difficult to predict, a good heuristic for the starting value of γ can be inferred by performing a sensitivity study on the optimal costate λ∗ (rs ) for various start/target locations for a given terrain. Figure 9 shows the result of running the polar algorithm on a reference terrain with various start lo27

cations and a single target location. For this exercise, γ was chosen as 0.0 for the outer loop initialisation. Figure 9(b) shows that the optimum costate varies almost consistently(increases) with the change in starting location and is not far away from the initial guess of γ = 0.0. Hence for all the simulations presented in this paper, γ = 0.0 is chosen as the initial guess. Threshold to switch over to inner loop ζ : A large value of parameter ζ can result in an initial condition far away from optimality for the Newton scheme, that may adversely affect the convergence rate of the loop. On the other hand a very small value of ζ may result in underutilisation of the inner loop at the expense of more outer loop iterations. In the simulations presented in this section, ζ = 0.2 is used Threshold for algorithm stop  : Ideally  should be 0.0 indicating optimality. But in a practical scenerio when the algorithm is executed in a robot, the bound on the extremal error |rf − rf∗ | will be the error in measurement in the inertial sensing unit. In the simulations, considering this uncertainty in measurement made by a typical inertial sensing unit  = 0.0001 is used for optimality assurance. Step size of the outer loop ∆ : A very large step size will result in overstepping a narrow solution region and a very small step size will increase the number of iterations drastically. In the simulations ∆ = 0.001 is used. Maximum number of iterations allowed for the outer loop M : Among all of the simulation cases considered, the maximum number of iterations of the inner loop was around 3000. Hence this parameter is chosen as 10000 accounting the uncertainty in the number of iterations in a random terrain. Maximum number of iterations allowed for the inner loop N : Among all of the simulation cases considered, the maximum number of iterations

28

of the inner loop was around 40. Hence this parameter is chosen as 100 accounting the uncertainty in the number of iterations in a random terrain. Maximum value of the costate variable at rs δ : This parameter is used for random selection of γ in case of non-convergence of the algorithm. γ is then chosen from a uniform distribution and scaled by δ. Hence δ should be sufficiently large to accommodate all the possible solution intervals. All of the simulations presented in this paper converged in the first run of the algorithm, hence a random guess of γ was not warranted. Nevertheless δ is set to 20.0 in the simulation runs. 5.2. Algorithm Convergence Newton-Raphson method has quadratic convergence in the neighbourhood of the optimal solution[18]. In order to ascertain the quadratic convergence of the algorithm numerically, the terrain shown in figure 9(a) is selected and the algorithm is executed with various start locations and fixed target location (33, 180o ). Figure 10 shows the graph of the number of iterations against the variation of the extremal |θ∗ (rf ) − θ(rf )|.

Figure 10: Comparison of Number of iterations v/s the error at the terminal state

29

For all the cases considered in figure 10, the initial guess for the algorithm γ was chosen as 0.0. The important inference from figure 10 is that irrespective of the number of iterations taken, the optimal path was found. It can also be observed from figure 10 that for certain starting locations viz. (4, 0o ) to (7, 0o ) the number of iterations are large. Starting locations (4, 0o ) to (7, 0o ) lies on the constricted region of the terrain where the control variable undergoes large change in slope. The solution space of the problem also gets constricted reducing the optimality neighbourhood leading to non-convergence of the Newton-Raphson method thereby increasing the outer loop execution rate. In order to emphasize the convergence rate of the algorithm for all the test cases, the algorithm was re-run with γ selected close to the optimum costate value for locations (4, 0o ) to (7, 0o )

Figure 11: Comparison of Number of iterations v/s the error at the terminal state

Figure 11 clearly shows the rate of convergence of the algorithm. The rate and order of convergence is quantified based on the criteria described in Fletcher et.al.[19]. Let en+1 < 1 be the error in the (n + 1)th iteration and let en < 1 be 30

the error in the nth iteration of a numerical algorithm. Then for convergence en+1 ≤ µepn

(26)

For quadratic convergence the order of convergence p = 2 and the rate of convergence µ > 0.0. Strong convergence is indicated when µ < 1.0. Computations on the results shown in figure 11 shows that the rate of convergence µ < 0.6 indicating a strong quadratic convergence. 5.3. Simulation Results

(a)

(b) Zw

(1,0)

Yw Xw

(c)

Target heading=45o

(d) (d)

Figure 12: (a)Path chosen on the terrain surface z = x2 + y 2 (3D view) (b)Contour plot of the optimal path. The initial point is (1, 0o ) in polar notation and the target location is (21.5, 45o ). The solid disc shows the initial and target location of the robot (c)Heading angle(θ) of the for the optimal path is in the order of 10−13 robot (d) ∂H ∂u

Figure 12(a) shows a 3D view of the paraboloid surface z = x2 + y 2 . The same terrain yielded an infeasible solution in the cartesian formulation. The robot is initially placed at (1, 0o ) in polar notation and directed to move to (21.5, 45o ) on the terrain as shown in figure 12(b) by the circles. Figure 12(a) 31

also shows the optimal path generated by the reformulated algorithm. The optimal path chosen by the algorithm is exactly the expected path indentified in figure 6. The contour plot shown in figure 12(b) confirms this observation. The system state θ∗ (r) and the control performance measure

∂H ∂u (in

the order

of 10−13 ) are shown in figures 12(c) and (d) respectively confirming optimality.

Zw (a)

(b)

(1,0)

Yw

Xw

(d)

Target heading=90o

(c)

(d)

Figure 13: (a)Path chosen on the terrain surface z = x2 + y 2 (3D view) (b)Contour plot of the optimal path. The initial point is (1, 0o ) in polar notation and the target location is (21.5, 90o ). The solid disc shows the initial and target location of the robot (c)Heading angle(θ) of the robot (d) ∂H for the optimal path is in the order of 10−13 ∂u

Figure 13 depicts the optimal path chosen by a robot positioned initially at (1, 0o ) in polar system and instructed to move to (21.5, 90o ). Figure 14 shows another example of the optimal path chosen by a robot positioned initially at (1, 180o ) in the world co-ordinate system and instructed to move to (21, 180o ). The respective figures show the plot of the robot heading θ∗ (r) as well as the measure of optimality

∂H ∂u .

The parameter

∂H ∂u

≈ 0.0 for all the examples con-

sidered indicating the optimality of the chosen path.

32

Zw (a)

(b)

(-1,0)

Yw

Xw

(d)

(c)

(d) Target heading=180o

Figure 14: (a)Path chosen on the terrain surface z = x2 + y 2 (3D view) (b)Contour plot of the optimal path. The initial point is (1, 180o ) in polar notation and the target location is (21, 180o ). The solid disc shows the initial and target location of the robot (c)Heading angle(θ) for the optimal pathis in the order of 10−13 of the robot (d) ∂H ∂u

33

(a)

Yw Robot direction when Root > 0.0

(b) Xw

Yw

Robot

Robot Robot direction when Root < 0.0

Xw

Positive root should be chosen Negative root should be chosen Target

Figure 15: (a)Illustration of positive and negative root as direction taken by the robot with respect to the world frame (b)Illustration of root selection for various target locations

The control u(r) should be selected as one of the roots of the quadratic equation 24. The significance of the sign of the roots was explained in section 3 as the heading direction(clockwise or anticlockwise), to be taken by the robot. Figure 15(a) illustrates the relationship between the heading and the sign of the root on the world frame reference and figure 15(b) illustrates the path taken by a robot based on selected root for various target locations. This illustrative example shows that a judicious selection of root is essential in determining the optimal path. Since the sign of the root is synonymous to the heading direction(clockwise/anticlockwise) the angular displacement of the target location from the starting location will provide a heuristic to the root selection problem. Let r1 and r2 represent the roots of the quadratic, then their signs will have the following combinations • Both r1 and r2 are positive. In this case, if both roots are not equal then trajectory has to be generated using both the roots and the minimum

34

length trajectory is chosen • Both r1 and r2 are negative. This is synonymous to the previous case • r1 and r2 have opposing sign. In this case, the initial position of the robot and the final position on the polar grid also should be considered for selecting the root as explained in table 5

Initial/Target 0− π 2

π 2

−π

π− 3π 2

3π 2

−0

0−

π 2

π 2

−π

π−

3π 2

3π 2

−0

-

+ve root

-ve root

-ve root

-ve root

-

+ve root

-ve root

-ve root

-ve root

-

+ve root

+ve root

+ve root

-ve root

-

Table 5: Root selection in the four quadrants

Figure 16 shows the sensitivity of the root selection on the performance of the algorithm. Here the initial position (5, 45o ) is in the first quadrant and the desired final position (25, 270o ) is in the third quadrant. Figures 16(a) and 16(b) shows the sub-optimal solution when the roots are not chosen as per table 5 and figures 16(c) and 16(d) shows the optimal solution when the root selection is executed as per table 5.

35

Zw (a)

(b)

(5,45o)

Yw

Xw (c)

Zw

(d)

(5,45o)

Yw

Xw

Figure 16: (a)Path chosen on the terrain surface z = x2 + y 2 (3D view) (b)Contour plot of the path. The initial point is (5, 45o ) in polar notation and the target location is (25, 270o ). Here the positive root of the quadratic is chosen that has resulted in sub-optimal solution (c)Path chosen on the terrain surface z = x2 + y 2 (3D view) (d)Contour plot of the optimal path when negative root of the quadratic is chosen.

The diagonal entries of table 5 correspond to cases wherein the initial and final positions lie in the same quadrant. Under these cases further investigations are required to select the optimum root as follows. Consider two line segments L1 and L2 both originating from the origin of the polar contour view of the terrain. L1 starts from the origin and terminates at the initial position of the robot and the line segment L2 starts from the origin and terminates at the desired final position. Let α be the angle between line segments L1 and L2 positive in the anti-clockwise sense measured from L1 . • if α is an acute angle, then the positive root should be selected • if α is an obtuse angle, then the negative root should be selected • if α = 0.0, then any one of the roots can be selected 36

In all the examples shown in this paper, the root selection(positive root or negative root) is exercised as an offline procedure based on the initial and final positions. The algorithm then always chooses the root with the desired sign throughout its execution. To establish the robustness of the algorithm, a semiclosed terrain feature that resemble a horse shoe was synthesised. The robot is placed inside this feature and the target locations are carefully selected to ensure that the robot has to first negotiate the narrow opening of the terrain formation and then retarget to the target location. Figure 17(a) shows the 3D view of the synthetic terrain in the world co-ordinate system and figure 17(b) shows the contour plot of the terrain in polar co-ordinate system. In this test case, the robot is initially placed at (1, 0o ) in polar notation and is directed to proceed to (21.5, 45o ). The elevation of the terrain formation is chosen such that any attempt to cross the formation will incur a large penalty to the robot. The robot chooses an optimal trajectory wherein it will first come out of the terrain formation and then retarget to the target location as expected. Figures 18, 19 and 20 shows the optimal path chosen for various target locations by the robot.

37

(a)

(b)

Yw

(1,0)

Zw Xw Target heading=45o

(d)

(c)

(d)

Figure 17: (a)Path chosen on the synthetic terrain (3D view) (b)Contour plot of the optimal path. The initial point is (1, 0o ) in polar notation and the target location is (21.5, 45o ). The solid disc shows the initial and target location of the robot (c)Heading angle(θ) of the robot (d) ∂H for the optimal pathis in the order of 10−13 ∂u

(a)

(b)

Yw

(1,0)

Zw Xw Target heading=90o

(d)

(c)

(d)

Figure 18: (a)Path chosen on the synthetic terrain (3D view) (b)Contour plot of the optimal path. The initial point is (1, 0o ) in polar notation and the target location is (21.5, 90o ). The solid disc shows the initial and target location of the robot (c)Heading angle(θ) of the robot (d) ∂H for the optimal pathis in the order of 10−13 ∂u

38

(a)

(b)

Yw

(1,0)

Zw Xw

(d)

(c) Target heading=135o

(d)

Figure 19: (a)Path chosen on the synthetic terrain (3D view) (b)Contour plot of the optimal path. The initial point is (1, 0o ) in polar notation and the target location is (21.5, 135o ). The solid disc shows the initial and target location of the robot (c)Heading angle(θ) of the robot (d) ∂H for the optimal pathis in the order of 10−13 ∂u

(a)

(b)

(3,0)

Yw

Zw Xw

(d) (c) (d)

Target heading=315o(-45o)

Figure 20: (a)Path chosen on the synthetic terrain (3D view) (b)Contour plot of the optimal path. The initial point is (3, 0o ) in polar notation and the target location is (22.5, 315o ). The solid disc shows the initial and target location of the robot (c)Heading angle(θ) of the robot (d) ∂H for the optimal pathis in the order of 10−13 ∂u

39

(a)

(b)

(9,45o)

Yw Zw Xw

(d) (c) (d) Target heading=180o

Figure 21: (a)Path chosen on the synthetic terrain (3D view) (b)Contour plot of the optimal path. The initial point is (5, 45o ) in polar notation and the target location is (31, 180o ). The solid disc shows the initial and target location of the robot (c)Heading angle(θ) of the robot for the optimal pathis in the order of 10−13 (d) ∂H ∂u

The mobility of the robot depends on the characteristics of the ground or terrain surface. Solid surface pose little problem to the robot whereas muddy or sandy surface may affect the locomotion. Moreover, liquid surfaces like water or corrosive acids should be avoided if the robot is not equipped with appropriate locomotion features to tread on such surfaces. In literatures pertaining to optimal motion planning, mobility is considered as a coefficient µm [7] that takes a value between 0 and 1. This term appears in the dynamic stability equation used to determine the maximum allowable velocity on the terrain. The mobility term can be incorporated in the minimum path length formulation presented in this paper without added complexity using penalty methods. The mobility coefficient can be considered proportional to the elevation of the terrain. The intractable regions on the terrain marked by values of µm close to 1 are replaced with terrain formations of infinite elevation that will incur a very large penalty 40

on the performance measure. Hence an optimal path will never include such formations. Figures 21(a) and (b) shows a terrain wherein the intractable regions are modelled as formations of infinite elevation. The robot is initially at (5, 45o ) in polar notation and is directed to move to (31, 180o ) avoiding the intractable regions on the terrain. Figures 21(a) and (b) also shows the path taken by the robot avoiding the formations to reach the target location. 5.4. Comparison with other optimal path planning algorithms

(a)

(b) Target Start Start Start Target Target

(c) Start

Target

Figure 22: (a)Case I: One obstacle in the way (b)Case II: No obstacle in the way but one obstacle near by (c)Case III: Two obstacles near to each other in the way

Haro et.al[20] in their work had compared the performance of 3 optimal path planning algorithms viz. Bug algorithm, Potential field algorithm and A∗ with Multiresolution. The authors have selected three test static terrains to analyse the performance of the above algorithms. The starting/target locations of the robot and the position of the obstacle(s) were tramsformed to the polar co-

41

ordinate system and suitable scaling factors were applied for easy visualisation. Figure 23 shows the result obtained for the test cases defined by Haro et.al[20]. (a)

(b)

(c)

Figure 23: (a)Case I: One obstacle in the way (b)Case II: No obstacle in the way but one obstacle near by (c)Case III: Two obstacles near to each other in the way

The polar co-ordinate formulated algorithm was able to find the optimal trajectory for all the three cases within 8 iterations. Comparison of the path optimality metric shows that the polar algorithm surpasses the potential field method that fails for case III and the A∗ with multi resolution in finding the optimal path. Figure 23 shows that for the polar algorithm the optimality measure

∂H ∂u

is in the order of 10−16 confirming the optimality of the chosen

path. Even though the performance of the bug algorithm is comparable to the polar algorithm, the authors have mentioned in their work that the bug algorithm may not always generate the optimal path as it is based on heuristics and geometrical assumptions. On the other hand, the polar algorithm is based on sound mathematical formulation with an optimality measure indicating the correctness of the solution and hence superior to the bug’s algorithm.

42

6. Conclusion A path planning algorithm based on path length minimisation was presented in this paper. The results shows that the migration from the cartesian system to the polar co-ordinate frame has improved the algorithm performance in providing feasible trajectories. The robustness of the algorithm was also demonstrated using several examples, along with the ability of the algorithm to represent tractability constraints. Even though, the b-spline surface is able to replicate the terrain on a global scale, local excursions in terrain geography should be addressed using an online guidance system that can make finer corrections to the trajectory on a real time basis.

7. References [1] E. W. Dijkstra, A note on two problems in connexion with graphs, Numerische Mathematik 1959,Vol.1,Issue 1,pp 269-271. [2] R. Bellman, On a routing problem, Quarterly of Applied Mathematics 1958,Vol.16,No.1,pp 87-90. [3] J. C. Latombe, Robot Motion planning, Kluwer Academic Publishers, 1991. [4] A. Elfes, Using occupancy grids for mobile robot perception and navigation, Journal of Computer 1989,Vol.22,Issue 6,pp-46-57 22 (6). [5] A. Stentz, Optimal and efficient path planning for partially-known environments, Proceedings of the IEEE International Conference on Robotics and Automation,1994. [6] B. Krogh, A generalized potential field approach to obstacle avoidance control, Proceedings of World Conference on Robotics Research, 1984. [7] Z. Schiller, Optimal motion planning of autonomous vehicles in three dimensional terrains, Proceedings of the IEEE International Conference on Robotics and Automation,1990.

43

[8] H. Delingette, M. Hebert, K. lkeuchi, Trajectory generation with curvature constraint based on energy minimization, proceedings of the International Workshop on Intelligent Robots and Systems, 1991. [9] J. Reuter, Mobile robots trajectories with continuously differentiable curvature: An optimal control approach, Proceedings of the International Conference on Intelligent Robots and Systems, 1998. [10] E. Frazzoli, M. A. Dahleh, E. Feron, Real-time motion planning for agile autonomous vehicles, Journal of Guidance, Control and Dynamics 2002,Vol 25, No.1,pp. 116-129. [11] D. Gaw, Minimum-time navigation of an unmanned mobile robot in a 21/2d world with obstacles, Proccedings of the IEEE International Conference on Robotics and Automation 1986. [12] C. F. Gerald, Applied Numerical Analysis, Pearson Education, 2008. [13] S. B. Ghosn, F. Drouby, H. M. Harmanani, A parallel genetic algorithm for the open-shop scheduling problem using deterministic and random moves, , International Journal of Artificial Intelligence, vol. 14, no. 1, pp. 130-144, 2016. [14] D. Martin, R. D. Toro, R. Harbor, J. Dorronsoro, Optimal tuning of a networked linear controller using a multi-objective genetic algorithm and its application to one complex electromechanical process, International Journal of Innovative Computing, Information and Control, vol. 5, no. 10 (B) pp. 3405-3414, 2009. [15] M. Reichardt, T. Fohst, K. Berns, An overview on framework design for autonomous robots, it - Information Technology, vol. 57, no. 2, pp. 75-84, 2015. [16] R. E. Precup, R. C. David, E. M. Petriu, S. Preti, M. B. Radac, Novel adaptive charged system search algorithm for optimal tuning of fuzzy con-

44

trollers, Expert Systems with Applications, vol. 41, no. 4, pp. 1168-1175, 2014. [17] D. E. Kirk, Optimal Control Theory An Introduction, Dover publications, 1970. [18] A. E. Bryson, Applied Optimal Control Optimisation, Estimation and Control, Taylor and Francis, 1975. [19] R. Fletcher, Practical Methods of Optimization, John Wiley Sons, 2003. [20] F. Haro, M. Torres, A comparison of path planning algorithms for omnidirectional robots in dynamic environments, Robots Symposium, LARS 2006.

45

Jinu Krishnan joined Vikram Sarabhai Space Centre, Thiruvananthapuram, India in 2008 and is presently working as scientist in Trajectory Modelling and Simulation division. He completed his post graduation in Computer Science and Engineering from Indian Institute of Science Bangalore. He is involved in the mathematical modelling and simulation of space transportation systems and design and development of robotic trajectory planning algorithms.

Dr. U.P. Rajeev is Head, Guidance Design Division of Vikram Sarabhai Space Center, Thiruvananthapuram, India. He completed M.Tech in Instrumentation and control systems from Regional Engineering College, Calicut, India in 1994 and joined Vikram Sarabhai Space Centre. He completed PhD from Indian Institute of Science Bangalore in 1998. From 1994 onwards he is working in the area of guidance algorithm development for space transportation systems. Areas of interest: optimal control and nonlinear control.

J Jayabalan joined in Vikram Sarabhai Space Centre in November 1983, obtained his M Tech in Computer Science & Engineering from Indian Institute of Technology, Madras. Presently he is heading Trajectory Modelling and Simulation Division and his area of interest include mathematical modelling and simulation of space transportation systems. He has several publication in Indian journals and conferences to his credit.

!1

Mrs.Sheela DS graduated in Electrical Engineering from College of Engineering, Thiruvananthapuram and obtained her postgraduate degree in Instrumentation and Control Systems from National Institute of Technology, Calicut. She is a pioneer in Launch vehicle guidance systems area. Currently she is the Group Director of Control and Guidance Design Group of Vikram Sarabhai Space Centre, Indian Space Research Organisation.

Research Highights April 14, 2017 1. In this paper, the path planning problem for a single autonomous robot treading on a static terrain is solved using optimal control theory. 2. The performance measure selected for the optimal control formulation is the path length between a predefined initial and final position on an inertial frame of reference. 3. The terrain features are assumed to be known a priori and is represented using a b-spline surface. The terrain tractability constraint is decoupled from the dynamic stability equation of the robot and absorbed as a terrain feature. 4. The results indicate that the target is reached optimally and the trajectory is smooth. The smoothness of the trajectory eliminates the need for post processing for trajectory tracking. 5. This formulation can be applied to path planning problems related to aerial and ground vehicles with minimum modifications to take care of the real time requirements.

1