In this tutorial, I show how to deal with obstacles in TO. Let's consider a robot moving in a 2D space. The robot needs to avoid rectangle obstacles. Here, I assume that the shape of the obstacles is represented by its lower left and upper right points, \(\left(x_{\min }, y_{\min }\right)\) and \(\left(x_{\max }, y_{\max }\right)\). Since we are interested in TO, the robot needs to avoid all obstacles over the planning time. I use \(i\) to represent time step. Then, for \(N\) steps TO, the constraints can be described as follows:
\(\begin{aligned} \forall i \in[1 \ldots, N]: \quad x_{i} & \leq x_{\min } \\ \text { or } x_{i} & \geq x_{\max } \\ \text { or } y_{i} & \leq y_{\min } \\ \text { or } y_{i} & \geq y_{\max } \end{aligned}\)
You may wonder how to consider the size of the robot, but it is easily soved. You can formulate these constraints in the configuration space.
The above constraints are not easy to implement because we do not know how to deal with "or". The key idea is that we can somehow convert these "or" constraints into "and" constraints. "as" constraints are much easier to formulate. We can realize this transformation using binary variables (that's why people like using Mixed-Integer Programming!). Let \(t_{ik}\) be a binary variable and let \(M\) be a large positive number. Therefore, we can have the following constraings from the above constraints:
\(\begin{aligned} \forall i \in[1, \ldots, N]: & x_{i} \leq x_{\min }+M t_{i 1} \\ \text { and } &-x_{i} \leq-x_{\max }+M t_{i 2} \\ \text { and } & y_{i} \leq y_{\min }+M t_{i 3} \\ \text { and } &-y_{i} \quad \leq-y_{\max }+M t_{i 4} \\ \text { and } & \sum_{k=1}^{4} t_{i k} \leq 3 \end{aligned}\)
Let me explain this equation. If the k-th original constraint is not satisfied at time \(i\), the corresponding variable \(t_{ik}\) is set to 1. For example, by setting \(t_{i1}\) to 1, \(x_{i} \leq x_{\min } + (\text{ very large number})\), resulting in that \(x_i\) can take arbitrary number or I can say that the upper limit of \(x_i\) is a very large number. And the final constraint in the equation ensures that at least one of these 4 linear constraints should be satisfied. This constraints can be formulated for every obstacles in the environment, for any convex shape obstacles, and for moving obstacles with easy modifications.
Reference
@inproceedings{Schouwenaars_2001, title={Mixed integer programming for multi-vehicle path planning}, url={http://dx.doi.org/10.23919/ECC.2001.7076321}, DOI={10.23919/ecc.2001.7076321}, booktitle={2001 European Control Conference (ECC)}, publisher={IEEE}, author={Schouwenaars, Tom and De Moor, Bart and Feron, Eric and How, Jonathan}, year={2001}, month=sep }