Soft constraints in MPC

[Jens Einar Bremnes]

Hi

I am struggling with formulating soft constraints in MATLAB using CasADi. I am wondering if anyone has experience with this?

For context, I want to specify time-varying state constraints for MPC for a mobile robot. Specifically, I want the allowable space that the robot can operate to shrink as time passes, due to reduced battery lifetime. As of now, I have implemented this as a hard constraint.

What currently happens in my simulations, is that when the robot goes outside the allowable space (due to the space shrinking), this space constraint is no longer satisfied, and I start getting some weird behavior (instead, I want the robot to start going toward the allowable space).

I think formulating this constraint as a soft constraint, that is, penalising it in the cost function, would be a good solution. However, I do not know the best way to do this in CasADi.

I could add a term min(0, 10000000* left hand side of constraint) int the cost function, such that the penalty is zero when the constraint is not violated, and a heavy penalisation when it is, but this cost function would be very discontinuous. 

Do you guys have any tips on the best way to solve this?

[Joris Gillis]

Dear Jens,

A fairly standard way, in the context of constraint programming, is to work with slack formulation.

Consider the path constraint h(x_k) <= 0  for k=1..N

Introduce s_k>=0 as extra decision variables

Modify your constraint as h(x_k) <= s_k,

and add a term with s in the objective: sum_k s_k^2 for a quadratic penalty (L2) or sum_k s_k for a linear penality (L1).

The L1 is particularly nice, you could even use it (given proper weights), to implement priorities for your different constraints..

Best regards,

  Joris

[Huckleberry Febbo]

Dear Joris,

what if we want to add a slack variable through the C++ API with an equality constraint?

e.g., starting with something like
x_k = x0;

adding a slack constraint
g.push_back(x_k + s_k);
g_min.insert(g_min.end(), x0.begin(), x0.end());
g_max.insert(g_max.end(), inf.begin(), inf.end());
where 
std::vector<double> inf = {casadi::inf};

then on the other side 
g.push_back(x_k - s_k);
g_min.insert(g_min.end(), neg_inf.begin(), neg_inf.end());
g_max.insert(g_max.end(), x0.begin(), x0.end());
where 
std::vector<double> neg_inf = {-casadi::inf};
then bound s_k to be greater than zero and add it to the cost functional.

would it be something like this?