I tookthe liberty to change somewhat the style of the coding, but the only meaningful change is in the variable constraintsm1, which was very deeply nested. However, the following code gives an answer very quiclky:
delta = 1/10; dt = 1/10; horizon = 20; x = 0;
y = 1/10; psi = 1/2; cx = 8; cy = 8; r = 3/2;
Ak[dt_, uv_, upsi_] := {{1, 0, dt*uv, 0},
{0, 1, 0, dt*uv},
{0, 0, Cos[dt*upsi], -Sin[dt*upsi]},
{0, 0, Sin[dt*upsi], Cos[dt*upsi]}};
Amom1[i_] := Ak[dt, uv[i], upsi[i]];
m1[0] = {{x}, {y}, {Cos[psi]}, {Sin[psi]}};
m1[i_] := Table[Subscript[a[i], k, l],
{k, 4}, {l, 1}];
uvArray = Array[uv, horizon];
upsiArray = Array[upsi, horizon];
List1 = Array[m1, horizon];
List2 = Table[Amom1[i] . m1[i - 1], {i, horizon}];
constraintsm1 = Thread[Flatten[List1] == Flatten[List2]];
vars = Join[uvArray, upsiArray, Flatten[List1]];
f[x_] := objectiveFunction; (* Objective function *)
PrintPartialResult[iteration_, vars_] :=
Print["Iteration ", iteration, ": f(x) = ",
f[vars], ", x = ", vars];
iteration = 1; (* Initialize iteration count *)
averageObstacleExpression[m1Aux_, obstacleExpression_] :=
obstacleExpression /. {x1 -> m1Aux[[1, 1]], x2 -> m1Aux[[2, 1]]};
squaredObstacleExpression = averageObstacleExpression;
averageObstacleAux2 = Expand[(x1 - cx)^2 + (x2 - cy)^2 - r^2];
constraintsaverageObstacle1 =
Table[-averageObstacleExpression[m1[i], averageObstacleAux2] <= 0,
{i, horizon}];
objectiveFunctionAux2 = Expand[(x1 - cx)^2 + (x2 - cy)^2];
squaredObstacleAux2 = Expand[((x1 - cx)^2 + (x2 - cy)^2 - r^2)^2];
constraintsaverageObstacle2 =
Table[(4/9 - delta) squaredObstacleExpression[m1[i],
squaredObstacleAux2] -
4/9 averageObstacleExpression[m1[i], averageObstacleAux2]^2 <=
0,
{i, horizon}];
objectiveFunction =
averageObstacleExpression[m1[horizon], objectiveFunctionAux2];
constraintsaverageObstacle3 =
Table[5/8 squaredObstacleExpression[m1[i], squaredObstacleAux2] -
averageObstacleExpression[m1[i], averageObstacleAux2]^2 <= 0,
{i, horizon}];
NMinimize[{objectiveFunction,
Thread[0 <= uvArray <= 20],
Thread[-Pi <= upsiArray <= Pi],
constraintsaverageObstacle1,
constraintsaverageObstacle2,
constraintsaverageObstacle3,
constraintsm1},
vars,
StepMonitor :> PrintPartialResult[iteration++, vars]]
Actually, the constraints constraintsaverageObstacle2 are superfluous, because they are all of the form k^2>=0.