One might encounter various frustrating difficulties when attempting to numerically solve a difficult nonlinear and nonconvex optimal control problem. In this text I’ll consider such a difficult problem, that of finding the shortest path between two points through an obstacle field for a widely known model of a wheeled robot. I’ll examine common issues that arise when trying to unravel such an issue numerically (particularly, nonsmoothness of the price and chattering within the control) and easy methods to address them. Examples help to make clear the concepts. Get all of the code here: https://github.com/willem-daniel-esterhuizen/car_OCP
1.1 Outline
First, I’ll introduce the automobile model that we’ll study throughout the article. Then, I’ll state the optimal control problem in all its detail. The following section then exposes all of the numerical difficulties that arise, ending with a “sensible nonlinear programme” that attempts to cope with them. I’ll then present the main points of a , which helps in guiding the solver towards a very good solution. I’ll then show you some numerical experiments to make clear every part, and finish off with references for further reading.
2. A automobile model
We’ll consider the next equations of motion,
[
begin{align}
dot x_1(t) &= u_1(t)cos(x_3(t)),
dot x_2(t) &= u_1(t)sin(x_3(t)),
dot x_3(t) &= u_2(t),
end{align}
]
where (t geq 0) denotes time, (x_1inmathbb{R}) and (x_2inmathbb{R}) denote the automobile’s position, (x_3inmathbb{R}) denotes its orientation, (u_1inmathbb{R}) its velocity and (u_2inmathbb{R}) its rate of turning. It is a common model of a , which consists of two wheels that may turn independently. This permits it to drive forwards and backwards, rotate when stationary and perform other elaborate driving manoeuvres. Note that, because (u_1) could be 0, the model allows the automobile to stop instantaneously.
Throughout the article we’ll let (mathbf{x} := (x_1, x_2, x_3)^topinmathbb{R}^3) denote the , (mathbf{u} := (u_1, u_2)inmathbb{R}^2) denote the , and define (f:mathbb{R}^3timesmathbb{R}^2 rightarrow mathbb{R}^3) to be,
[
f(mathbf{x},mathbf{u}) :=
left(
begin{array}{c}
u_1cos(x_3)
u_1sin(x_3)
u_2
end{array}
right),
]
in order that we are able to succinctly write the system as,
[
dot{mathbf{x}}(t) = f(mathbf{x}(t), mathbf{u}(t)),quad tgeq 0.
]
3. Optimal path planning problem
Considering the automobile model within the previous section, we wish to. To that end, we’ll consider the next optimal control problem:
[
newcommand{u}{mathbf{u}}
newcommand{x}{mathbf{x}}
newcommand{PC}{mathrm{PC}}
newcommand{C}{mathrm{C}}
newcommand{mbbR}{mathbb{R}}
newcommand{dee}{mathrm{d}}
newcommand{NLP}{mathrm{NLP}}
mathrm{OCP}:
begin{cases}
minlimits_{x, u} quad & J(x, u)
mathrm{subject to:}quad & dot{x}(t) = f(x(t),u(t)), quad & mathrm{a.e.},, t in[0,T], &
quad & x(0) = x^{mathrm{ini}}, & &
quad & x(T) = x^{mathrm{tar}}, & &
quad & u(t) in mathbb{U}, quad & mathrm{a.e.},, t in[0,T], &
quad & (x_1(t) – c_1^i)^2 + (x_2(t) – c_2^i)^2 geq r_i^2, quad & forall tin[0,T],, forall iin mathbb{I}, &
quad & (x, u) in C_T^3timesPC_T^2,
end{cases}
]
where (Tgeq 0) is the finite time horizon, (J:C_T^3timesPC_T^2rightarrow mbbR_{geq 0}) is the price functional, (x^{mathrm{ini}} inmbbR^3) is the initial state and (x^{mathrm{tar}}inmbbR^3) is the goal state. The control is constrained to (mathbb{U}:= [underline{u}_1, overline{u}_1]times [underline{u}_2, overline{u}_2]), with (underline{u}_j < 0 [ I’ve use the short-hand (PC_T^m) (respectively (C_T^m)) to indicate all piece-wise continuous functions (respectively continuous functions) mapping the interval ([0,T]) to (mbbR^m). The acronym “(mathrm{a.e.},, t in[0,T])” stands for “almost every t in ([0,T])”, in other words, the derivative may not exist at a finite variety of points in ([0,T]), for instance, where the control is discontinuous. The equations of motion and the presence of obstacles make the optimal control problem nonlinear and nonconvex, which is difficult to unravel on the whole. A solver may converge to solutions, which aren’t necessarily globally optimal, or it could fail to search out a feasible solution regardless that one exists. For simplicity the obstacles are circles. These are nice because they’re differentiable, so we are able to use a gradient-based algorithm to unravel the OCP. (We’ll use IPOPT, which implements an interior-point method.) The arc length function, (J), is differentiable when the automobile’s speed is zero. Nevertheless, as we’ll see, we are able to eliminate this problem by adding a small (varepsilon > 0) under the square-root. Depending on the scheme used to discretise the equations of motion, there could also be within the control signal. As we’ll see, this could easily be handled by penalising excessive control motion in the price function. The horizon length considered in the issue, (T), is fixed. Thus, as the issue is posed, we actually want to search out the curve of shortest arc length for which the automobile reaches the goal in precisely (T) seconds. This is definitely not a difficulty for our automobile because it may stop instantaneously and rotate on the spot. So, the solutions to the OCP (if the solver can find them) might consist of long boring chunks firstly and/or end of the time period if (T) could be very large. If we desired to make the horizon length a call variable in the issue then we have now two options. First, if we use a to numerically solve the issue (as we’ll do in the subsequent section) we could vary the horizon length by making the discretisation step size (h), which appears within the numerical integration scheme, a call variable. It’s because the dimension of the choice space have to be set on the time at which you invoke the numerical nonlinear programme solver. The second option is to resort to an (in a nutshell, it is advisable to solve a boundary-value problem that you simply get from an evaluation of the issue via Pontryagin’s principle, often via the ). Nevertheless, for an issue like ours, where you have got many state constraints, this could be quite difficult. For those who are finding this text interesting, please consider testing the blog topicincontrol.com. It presents deep dives into control theory, optimization, and related topics, often with freely available code. We’ll solve the optimal control problem using . More precisely, we’ll take the control to be piecewise constant over a uniform grid of time intervals and propagate the state trajectory from the initial condition using the fourth-order Runge-Kutta (RK4) method. We’ll then form a finite-dimensional nonlinear programme (NLP), where the choice variables consist of the state and control at each discrete time step. This section shows easy methods to form a “sensible” NLP, which deals with the assorted numerical difficulties. Consider the automobile’s differential equation, with initial condition, (x^{mathrm{ini}}), over an interval, ([0,T]), [ Let (h>0) denote the constant time step, and let (K := mathrm{floor}(T/h)). Then the RK4 scheme reads, [ where (mathrm{RK}_4:mbbR^3times mbbR^2 rightarrow mbbR^3) reads, [ and [ The notation (x[k]) and (u[k]) is supposed to indicate the discretised state and control, respectively, in order to differentiate them from their continuous-time counterparts. You is likely to be tempted to contemplate the as the price within the NLP, namely, [ Nevertheless, this function is differentiable if for some (kin{0,1,dots,K-1}), [ which frequently results in the solver failing. You would possibly see the error One solution is to approximate the polygonal arc length with, [ with (varepsilon > 0) a small number. We see that, for an arbitrary (kin{1,dots,K-1}) and (iin{1,2}), [ and so this function is repeatedly differentiable, ensuring smooth gradients for IPOPT. (You get similar expressions should you take a look at (frac{partial}{partial x_i[K]}) and (frac{partial}{partial x_i[0]})). is the rapid jumping/oscillation/switching of the optimal control signal. There could also be parts of the answer that actually chatter (this may occasionally occur when the optimal control is , for instance) or a numerical solver may find an answer that chatters artificially. Delving into this deep topic is out of this text’s scope but, very briefly, you may encounter this phenomenon in problems where the optimal solution exhibits so-called . These are portions of the answer along which the state constraints are lively, which, in our setting, corresponds to the automobile travelling along the boundaries of the obstacles along its optimal path. When solving problems exhibiting such arcs via a , as we’re doing, the numerical solver may approximate the true solution along these arcs with rapid oscillation. Thankfully, a straightforward approach to eliminate chattering is to only penalise control motion by adding the term: [ in the price function, for some small (delta). (This not less than works well for our problem, even for very small (delta).) A well-scaled nonlinear programme is one where small changes in the choice variable end in small changes in the price and the values of the constraint functions (the so-called ). We are able to check how well our problem is scaled by taking a look at the magnitude of the price function, at its gradient and Hessian, as well the Jacobian of the constraint function at points in the choice space (in particuar, on the initial warm start and points near the answer). If these quantities are of comparable order then the solver might be robust, meaning it should often converge in relatively few steps. A badly-scaled problem may take extremely long to converge (because it would must take very small steps of the choice variable) or just fail. Considering our problem, it is smart to scale the price by roughly the length we expect the ultimate path to be. selection is the length of the road connecting the initial and goal positions, call this (L). Furthermore, it then is smart to scale the constraints by (1/L^2) (because we’re squaring quantities here). Taking the discussions of the previous subsections into consideration, the sensible NLP that we’ll consider within the numerics section reads, [ where (J_{varepsilon,delta}: (mbbR^{3})^K times (mbbR^{2})^{K-1} rightarrow mbbR_{>0}) is defined to be, [ To help upcoming discussion, define the , (Omega), to be the set of all pairs ((x,u)) that satisfy the constraints of (NLP_{varepsilon, delta}). A feasible pair ((x^{(varepsilon,delta)}, u^{(varepsilon,delta)})) is claimed to be provided that, [ A feasible pair ((x^{(varepsilon,delta)}, u^{(varepsilon,delta)})) is claimed to be provided that there exists a sufficiently small (gamma >0) for which, [ Here (mathbf{B}_{gamma}(x^{(varepsilon,delta)}, u^{(varepsilon,delta)}) ) denotes a Euclidean ball of radius (gamma>0) centred in regards to the point ((x^{(varepsilon,delta)}, u^{(varepsilon,delta)}) ). Recall that the issue is nonlinear and nonconvex. Thus, if we were to only select a small (varepsilon>0) and (delta>0) and throw (NLP_{varepsilon, delta}) at a solver, then it could fail regardless that feasible pairs might exist, or it could find “bad” locally optimal pairs. One approach to cope with these issues is to by successively solving easier problems that converge to the difficult problem. That is the concept behind using a . We are going to guide the solver towards an answer with a straightforward algorithm that successively gives the solver a very good warm start: Homotopy algorithm [ NOTE!: The homotopy algorithm is supposed to assist the solver find a very good solution. Nevertheless, there may be guarantee that it should successfully find even a feasible solution, nevermind a globally optimal one. For some theory regarding so-called you may seek the advice of the papers, [1] and [2]. On this section we’ll consider (NLP_{varepsilon, delta}) with initial and goal states (x^{mathrm{ini}} = (2,0,frac{pi}{2})) and (x^{mathrm{tar}} = (-2,0,mathrm{free})), respectively (thus (L = 4)). We’ll take the horizon as (T=10) and keep the discretisation step size within the RK4 scheme constant at (h=0.1), thus (K=100). Because the control constraints we’ll take (mathbb{U} = [-1,1] times [-1, 1]), and we’ll consider this interesting obstacle field: First, we’ll check the issue’s scaling at the nice and cozy start. Stack the state and control into an extended vector, (mathbf{d}inmbbR^{3K + 2(K-1)}), as follows: [ and write (NLP_{varepsilon, delta}) as: [ where (g) collects all of the constraints. As a warm start, call this vector (dini), we’ll take the road connecting the initial and goal positions, with (u_1[k] equiv 0.1) and (u_2[k] equiv 0). We should always have a take a look at the magnitudes of the next quantities at the purpose (dini): We are able to do that with the code from the repo: === SCALING DIAGNOSTICS === Objective J : 1.031e+00 ||∇J||_2 : 3.430e-01 ||Hess J||_Frob : 1.485e+02 ||g||_2 : 7.367e+00 ||Jac g||_Frob : 2.896e+01 ============================ With small (varepsilon) and (delta), and with our chosen (dini), the target should obviously be near 1. The dimensions of its gradient and of the constraint residuals at the nice and cozy start ought to be of comparable order. Within the print-out, (Vert nabla J Vert_2) denotes the Euclidean norm. The fee function’s Hessian could be of larger order (this increases with a finer time step). Within the print-out, (Vert nabla mathrm{Hess} J Vert_{mathrm{Frob}}) is the Frobenius norm of the Hessian matrix, which, intuitively speaking, tells you “how big” the linear transformation is “on average”, so it’s a very good measure to contemplate. The Jacobian of (g) will also be of barely larger order. The print-out shows that our problem is well-scaled, great. But one very last thing we’ll also do is tell IPOPT to scale the issue before we solve it, with This subsection shows the effect of the parameters (varepsilon) and (delta) on the answer, without running the homotopy algorithm. In other words, we fix (varepsilon) and (delta), and solve (NLP_{varepsilon, delta}) straight, without even specifying a very good initial guess. Here is an example of how you need to use the article’s repo to unravel an issue: Note how the solver might find locally optimal solutions which might be clearly not so good, like within the case (varepsilon) = (delta) = 1e-4. That is one motivation for using homotopy: you would guide the solver to a greater locally optimal solution. Note how (delta) affects the chattering, with it being really bad when (delta=0). The solver just fails when (varepsilon=0). Let’s now solve the issue with the homotopy algorithm. I.e., we iterate over (i) and feed the solver the previous problem’s solution as a warm start on each iteration. The initial guess we offer (at (i=0)) is similar one we used after we checked the conditioning, that’s, it’s just the road connecting the initial and goal positions, with (u_1[k] equiv 0.1) and (u_2[k] equiv 0). The figures below show the answer obtained at various steps of the iteration within the homotopy algorithm. The parameter (delta) is kept above 1e-4, so we don’t have chattering. As one would expect, the arc length of the answer obtained via the homotopy algorithm monotonically decreases with increasing (i). Here, (delta)=1e-4 throughout. I’ve considered a difficult optimal control problem and passed through the steps one must follow to unravel it intimately. I’ve shown how one can practically cope with problems with nondifferentiability and control chattering, and the way a straightforward homotopy method can result in solutions of greater quality. A classic book on practical issues surrounding optimal control is [3]. Seek the advice of the papers [4] and [5] for well-cited surveys of numerical methods in optimal control. The book [6] is one other excellent reference on numerical methods. [1] Watson, Layne T. 2001. “Theory of Globally Convergent Probability-One Homotopies for Nonlinear Programming.” 11 (3): 761–80. https://doi.org/10.1137/S105262349936121X. [2] Esterhuizen, Willem, Kathrin Flaßkamp, Matthias Hoffmann, and Karl Worthmann. 2025. “Globally Convergent Homotopies for Discrete-Time Optimal Control.” 63 (4): 2686–2711. https://doi.org/10.1137/23M1579224. [3] Bryson, Arthur Earl, and Yu-Chi Ho. 1975. . Taylor; Francis. [4] Betts, John T. 1998. “Survey of Numerical Methods for Trajectory Optimization.” 21 (2): 193–207. [5] Rao, Anil V. 2009. “A Survey of Numerical Methods for Optimal Control.” 135 (1): 497–528. [6] Gerdts, Matthias. 2023. . Walter de Gruyter GmbH & Co KG.
J(x,u) = int_0^Tleft(dot x_1^2(s) + dot x_2^2(s)right)^{frac{1}{2}}, dee s.
]3.1 Some comments on the OCP
4. Deriving a wise nonlinear programme
4.1 The RK4 scheme
dot{x}(t) = f(x(t), u(t)), quad tin[0,T], quad x(0) = x^{mathrm{ini}}.
]
x[k+1] = x[k] + frac{h}{6}mathrm{RK}_4(x[k], u[k]), quad forall ,, k in[0:K-1], quad x[0] = x(0),
]
mathrm{RK}_4(x, u) = k_1 + 2k_2 + 2k_3 + k_4,
]
k_1 = f(x, u),quad k_2 = f(x+ frac{h}{2}k_1, u), quad k_3 = f(x + frac{h}{2}k_2, u),quad k_4 = f(x + hk_3, u).
]4.2 Singularity of the price functional
sum_{k=0}^{K-1}Vertx[k+1] – x[k]Vert = sum_{k=0}^{K-1}left((x_{1}[k+1] – x_1[k])^2 + (x_2[k+1] – x_2[k])^2right)^{frac{1}{2}}.
]
Vertx[k+1] – x[k]Vert = 0,
]EXIT: Invalid number in NLP function or derivative detected should you solve an issue with this text’s code (which uses IPOPT, a gradient-based solver).
sum_{k=0}^{K-1}left((x_{1}[k+1] – x_1[k])^2 + (x_2[k+1] – x_2[k])^2 + varepsilonright)^{frac{1}{2}},
]
begin{align*}
frac{partial}{partial x_i[k]} &left( sum_{m=0}^{K-1} left(x_1[m+1] – x_1[m])^2 + (x_2[m+1] – x_2[m])^2 + varepsilonright)^{frac{1}{2}} right) [6pt]
&= frac{x_i[k] – x_i[k-1]}{left((x_1[k] – x_1[k-1])^2 + (x_2[k] – x_2[k-1])^2 + varepsilonright)^{frac{1}{2}}}
&quad + frac{x_i[k] – x_i[k+1]}{left((x_1[k+1] – x_1[k])^2 + (x_2[k+1] – x_2[k])^2 + varepsilon right)^{frac{1}{2}}}
end{align*}
]4.3 Control chattering
deltasum_{k=0}^{K-1}Vert u[k] Vert^2
]4.4 Scaling for good numerical conditioning
4.5 The sensible nonlinear programme
NLP_{varepsilon, delta}:
begin{cases}
minlimits_{x, u} quad & J_{varepsilon,delta}(x, u)
mathrm{subject to:}
quad & x[k+1] = x[k] + frac{h}{6}mathrm{RK}_4(x[k], u[k]), & forall k in[0:K-1], &
quad & x[0] = x^{mathrm{ini}}, &
quad & x[K] = x^{mathrm{tar}}, &
quad & u[k]in mathbb{U}, & forall k in[0:K-1], &
quad & frac{1}{L^2}left( x_1[k] – c_1^i right)^2 + frac{1}{L^2}left( x_2[k] – c_2^i right)^2 geq frac{1}{L^2} r_i^2, quad & forall k in[0:K], & forall iin mathbb{I},
quad & (x, u) in (mbbR^{3})^K times (mbbR^{2})^{K-1}, &
end{cases}
]
J_{varepsilon,delta}(x, u) := frac{1}{L} left(sum_{k=0}^{K-1}left(Vert x[k+1] – x[k]Vert^2 + varepsilon right)^{frac{1}{2}} + deltasum_{k=0}^{K-1}Vert u[k] Vert^2 right).
]
J_{varepsilon,delta}(x^{(varepsilon,delta)}, u^{(varepsilon,delta)}) leq J_{varepsilon,delta}(x, u),quadforall(x, u) in Omega.
]
J_{varepsilon,delta}(x^{(varepsilon,delta)}, u^{(varepsilon,delta)}) leq J_{varepsilon,delta}(x, u),quadforall(x, u) in Omegacap mathbf{B}_{gamma}(x^{(varepsilon,delta)}, u^{(varepsilon,delta)}).
]
5. A homotopy method
begin{aligned}
&textbf{Input:}text{ Initial parameters } varepsilon_{mathrm{ini}}>0, delta_{mathrm{ini}}>0. text{ Tolerances } mathrm{tol}_{varepsilon}>0, mathrm{tol}_{delta}>0.
&textbf{Output:} text{ Solution } (x^{(mathrm{tol}_{varepsilon}, mathrm{tol}_{delta})}, u^{(mathrm{tol}_{varepsilon}, mathrm{tol}_{delta})}) text{ (if it can find one)}
&quad text{Get initial warm start}, (x^{mathrm{warm}}, u^{mathrm{warm}}) text{ (not necessarily feasible)}
&quad i gets 0
&quad textbf{ While } varepsilon > mathrm{tol}_{varepsilon} text{ and } delta > mathrm{tol}_{delta}:
&quadquad varepsilongets max { varepsilon_{mathrm{ini}} / 2^i, mathrm{tol}_{varepsilon} }
&quadquad deltagets max { delta_{mathrm{ini}} / 2^i, mathrm{tol}_{delta} }
& quadquad text{ Solve } NLP_{varepsilon, delta} text{ with warm start } (x^{mathrm{warm}}, u^{mathrm{warm}})
& quadquad text{Label the solution } (x^{(varepsilon, delta)}, u^{(varepsilon, delta)}).
&quadquad (x^{mathrm{warm}}, u^{mathrm{warm}})gets (x^{(varepsilon, delta)}, u^{(varepsilon, delta)}).
&quadquad igets i + 1
&textbf{end while}
&textbf{return}, (x^{(mathrm{tol}_{varepsilon}, mathrm{tol}_{delta})}, u^{(mathrm{tol}_{varepsilon}, mathrm{tol}_{delta})})
end{aligned}
]
6. Numerical experiments
6.1 Checking conditioning
mathbf{d} := (x_1[0],x_2[0],x_3[0],x_1[1],x_2[1],x_3[1],dots,u_1[0],u_2[0],u_1[1],u_2[1],dots,u_1[K-1],u_2[K-1]),
]
newcommand{d}{mathbf{d}}
newcommand{dini}{mathbf{d}^{mathrm{ini}}}
begin{cases}
minlimits_{mathbf{d}} quad & J_{varepsilon,delta}(d)
mathrm{subject to:}
quad & g(d) leq mathbf{0},
end{cases}
]
import numpy as np
from car_OCP import get_initial_warm_start, build_and_check_scaling
x_init = np.array([[2],[0],[np.pi/2]])
x_target = np.array([[-2],[0]])
T = 10 # Horizon length
h = 0.1 # RK4 time step
obstacles = [
{'centre': (0,0), 'radius': 0.5},
{'centre': (-0.5,-0.5), 'radius': 0.2},
{'centre': (1,0.5), 'radius': 0.3},
{'centre': (1,-0.35), 'radius': 0.3},
{'centre': (-1.5,-0.2), 'radius': 0.2},
{'centre': (1.5,-0.2), 'radius': 0.2},
{'centre': (-0.75,0), 'radius': 0.2},
{'centre': (-1.25,0.2), 'radius': 0.2}
]
constraints = {
'u1_min' : -1,
'u1_max' : 1,
'u2_min' : -1,
'u2_max' : 1,
}
warm_start = get_initial_warm_start(x_init, x_target, T, h)
build_and_check_scaling(x_init, x_target, obstacles, constraints, T, h, warm_start, eps=1e-4, delta=1e-4)"ipopt.nlp_scaling_method":.... arrange problem ....
opts = {"ipopt.print_level": 0,
"print_time": 0,
"ipopt.sb": "yes",
"ipopt.nlp_scaling_method": "gradient-based"}
opti.minimize(cost)
opti.solver("ipopt", opts)6.2 Solving WITHOUT homotopy
import numpy as np
from car_OCP import solve_OCP, get_arc_length, plot_solution_in_statespace_and_control
eps=1e-2
delta=1e-2
x_init = np.array([[2],[0],[np.pi/2]])
x_target = np.array([[-2],[0]])
T = 10 # Horizon length
h = 0.1 # RK4 time step
obstacles = [
{'centre': (0,0), 'radius': 0.5},
{'centre': (-0.5,-0.5), 'radius': 0.2},
{'centre': (1,0.5), 'radius': 0.3},
{'centre': (1,-0.35), 'radius': 0.3},
{'centre': (-1.5,-0.2), 'radius': 0.2},
{'centre': (1.5,-0.2), 'radius': 0.2},
{'centre': (-0.75,0), 'radius': 0.2},
{'centre': (-1.25,0.2), 'radius': 0.2}
]
constraints = {
'u1_min' : -1,
'u1_max' : 1,
'u2_min' : -1,
'u2_max' : 1,
}
x_opt, u_opt = solve_OCP(x_init, x_target, obstacles, constraints, T, h, warm_start=None, eps=eps, delta=delta)
plot_solution_in_statespace_and_control(x_opt, u_opt, obstacles, arc_length=np.round(get_arc_length(x_opt), 2), obstacles_only=False, eps=eps, delta=delta)
6.3 Solving WITH homotopy
7. Conclusion
8. Further reading
References
