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
.