# How to speed convergence when optimizing a linear objective with nonlinear constraints?

I'm trying to learn how to do optimal rocket trajectory planning. I have a process that works but it converges very slowly; I'm looking for help to understand how to speed that up.

## The optimal control problem

This is a simplified version of the linear tangent steering problem:

Imagine a point mass (the "rocket") moving in a 2D plane. No gravity or other external force acts on it. The rocket accelerates itself (up to a maximum), but for now at least it does not change mass. It can instantly rotate to face in any direction.

The goal is to get from a known initial state to a partially-specified final state in minimum time without accelerating above the allowed limit. For my example:

• Initial position is at the origin
• Initial velocity is zero
• Final position is specified in the Y direction (height)
• Final position is unspecified in the X direction (down-range)
• Final velocity is zero in the Y direction (vertical)
• Final velocity is specified in the X direction (horizontal)
• Maneuver duration is unspecified (it's the variable being optimized)

## Discretizing the problem

I'm breaking the trajectory up into N segments of equal duration. In each segment the rocket has a constant acceleration vector, so the optimal trajectory is approximated by a series of parabolic arcs put end-to-end, with matching positions and velocities at the boundaries between them.

The general approach I'm taking is to start with N very small; say two or three segments. Once I've converged on a solution I can double the number of segments by halving the segment duration and duplicating each segment. Then I iteratively improve that solution until it converges, and repeat.

The problem is simple enough that there are a variety of ways of representing the state and constraints. It's possible to just do the integration in closed form instead of having segment integration constraints, for instance, which gives you a very small number of constraints for the final conditions, plus all the acceleration constraints for each of the segments.

You could opt to connect the segments together with integration constraints instead (specifying that the velocity at the end of a parabolic arc should be consistent with the acceleration over the arc, and that the position should be consistent with the initial and final velocities on that segment). This is more typical and would enable you to go on and add gravity and so forth more readily.

Accelerations could be an explicit part of the state vector, or they could be derived by comparing the initial and final velocity of each segment.

## Solving the problem

I'm solving this by taking a candidate solution and alternating two steps:

• Move the solution in the direction of the constrained gradient by some amount
• Move the solution toward the feasibility boundary if it's violating constraints

$$\begin{eqnarray*} \vec{x} &=& \text{state vector} \\ \vec{g} &=& \text{objective direction (the unconstrained objective gradient)} \\ \vec{n}_0, \vec{n}_1, \dotsc &=& \text{constraint directions} \\ c_0, c_1, \dotsc &=& \text{constraint errors} \\ \lambda_0, \lambda_1, \dotsc &=& \text{constraint multipliers} \\ \vec{p} &=& \text{constrained movement direction} \end{eqnarray*}$$

The $\vec{g}$ vector is just $-1$ in the duration variable and $0$ everywhere else, since I'm just trying to minimize time. The constraint normals $\vec{n}_i$ and errors $c_i$ are evaluated at the current state $\vec{x}$.

We want the movement direction $\vec{p}$ to be perpendicular to all active constraint normals, which we accomplish by subtracting out the parts of $\vec{g}$ that are not perpendicular to the constraints:

$$\begin{eqnarray*} \vec{p} &=& \vec{g} - \lambda_0 \vec{n}_0 - \lambda_1 \vec{n}_1 - \dotsb - \lambda_m \vec{n}_m \\ \vec{p}^\intercal \vec{n}_0 &=& 0 \\ \vec{p}^\intercal \vec{n}_1 &=& 0 \\ \vdots \\ \vec{p}^\intercal \vec{n}_m &=& 0 \end{eqnarray*}$$

Substituting the equation for $\vec{p}$ into the $m$ equations for perpendicularity lets us solve for the constraint normal multipliers $\lambda_i$, which can then in turn be put into the first equation to yield $\vec{p}$.

## Where I'm stuck

The next question is how far to move. $\vec{p}$ can be used as-is, but the constraints for this sort of problem are all essentially spherical, so as we reach the optimum they flatten out and movement slows to a crawl. I've tried scaling $\vec{p}$ up by an arbitrary amount but that doesn't help much, and seems inelegant.

I can think of a couple of possible things to do. I know what the expected rate of objective change will be, by doing a dot product between $\vec{g}$ and $\vec{p}$. If I could get a second derivative for the expected objective change I could guess how long of a step to take to reach the minimum. I'm getting all tangled up trying to figure out how to do that, though.

An even closer fit might be to get an entire acceleration vector for movement in the constrained gradient direction, so that we'd move along a parabola instead of a straight line. This would keep the solution closer to the feasibility surface, so the feasibility correction step would have less to do. I'm not too worried about that, though; the feasibility restoration step works pretty well.

Am I headed in the right direction for solving this? Is there a better way? Are there any good references for this sort of problem?