*5.4. Constraints*

The constraints added relate to limitations of the vehicle, such as maximum drive velocities, but also define harder acceleration limits to obtain a softer and smoother driving characteristic. The cost objective defined in Section 5.3 pulls all states towards the goal, while the constraints limit the velocity profile. The constraints are also used to handle obstacle avoidance by the planner.

#### 5.4.1. Velocity Profiles and Vehicle Constraints

The constraints utilized are to ensure strict boundaries as max velocities and accelerations are not exceeded. Constraints are also used to ensure that the generated trajectory is consistent; that is, that the states' progression by applying the corresponding control action will bring the current state to the next. In principle it would be sufficient to provide the initial state and the generated control action to derive the trajectory, but this work utilizes a multiple-shooting [34] approach where we keep track of the states in each iteration which is useful as some constraints added contains state information (note that the control output sent to the vehicle is defined in the state).

Given a control action **u** = [*dv*, *dϕ*, *dω*] the next state is computed by integrating the vehicle dynamics *f* specified in Equation (14), using Runge–Kutta RK4 for the integration. The equality constraint used to make sure the state integrated with control is consistent with the next state **s***k*+<sup>1</sup> = *F*(**s***k*, **u***k*, *dt*) is defined by :

$$\begin{aligned} k\_1 &= f(\mathbf{s}\_k, \mathbf{u}\_k) \\ k\_2 &= f(\mathbf{s}\_k + k\_1 \frac{dt}{2}, \mathbf{u}\_k) \\ k\_3 &= f(\mathbf{s}\_k + k\_2 \frac{dt}{2}, \mathbf{u}\_k) \\ k\_4 &= f(\mathbf{s}\_k + k\_3 dt, \mathbf{u}\_k) \\ \mathbf{s}\_{k+1} &= \mathbf{s}\_k + (k\_1 + 2k\_2 + 2k\_3 + k\_4) \cdot dt \end{aligned} \tag{20}$$

To limit the control actions as well as the maximum velocities, the following nonequality constraints are added; also see Equation (11):

$$\begin{cases} -d\upsilon^{\text{max}}dt \le d\upsilon \le d\upsilon^{\text{max}}dt\\ -d\varrho^{\text{max}}dt \le d\varphi \le d\varphi^{\text{max}}dt\\ -d\omega^{\text{max}}dt \le d\omega \le d\omega^{\text{max}}dt \end{cases} \tag{21}$$

To ensure that the maximum drive velocity on each wheel is not exceeded, the following constraints are utilized:

$$\begin{aligned} -\omega^{\text{max}} \le \omega - v/d \le \omega^{\text{max}}\\ -\omega^{\text{max}} \le \omega + v/d \le \omega^{\text{max}} \end{aligned} \tag{22}$$

Here, *d* is the distance between the centre of the platform and the wheels, which is assumed to be the same for all wheels.

The above two sets of constraints are used on all control variables in the optimization.

### 5.4.2. Obstacle Constraints

As one of the objectives is to drive in confined areas it is necessary to have good spatial resolution of obstacles. Rather than using a grid-based representation that would discretize the environment and hence lower the available resolution, instead we use range readings directly to have as high resolution as possible and to receive quick feedback. This requires that we have a good overview of the surrounding of the platform at all times. In essence, an obstacle **o** = [*ox*, *oy*] is a point in 2D.

Note that the global path planner will, to a great extent, handle obstacle avoidance while providing the global path. However, the obstacle constraints are necessary to safely drive at close proximity to obstacles.

From the raw range readings we process the data to only extract the key LiDAR readings. This is essential as each constraint will add additional complexity to the optimization problem at hand. How the obstacle constraints are formulated is also greatly affected by the representation used to model the platform. Due to the square shape of the platform the orientation is essential. For example, to drive through a narrow passage straight ahead along the x-axis, the orientation *θ* must be kept close to the following angles (0, *<sup>π</sup>*/2, *<sup>π</sup>*, 3*π*/4) as the required width would be approximately <sup>√</sup><sup>2</sup> larger if we instead had an orientation of *π*/4.

As the optimization problem can be altered on the fly, there are two different shapes that are used. One is used for driving, and thus we want the platform to drive with a specific forward direction, and the other is used for close-proximity driving when reaching goals. In the first scenario we define the footprint as two circles, one slightly forward and one slightly backwards. The front circle and back circle will stick out and make the footprint form in a clear direction, which is the best way to pass a narrow passage. The front circle will act as a "plow" that divides obstacles on either side. For close proximity near a goal, we instead approximate the shape of the robot better using five circles, one center and the remainder used to cover the corners of the platform.

To approximate the platform using only a circle would simplify the computation of the constraint to be the following:

$$\sqrt{\left(\mathbf{x}\_{k} - \mathbf{o}\_{\mathbf{x}}\right)^{2} + \left(y\_{k} - \mathbf{o}\_{\mathbf{y}}\right)^{2}} \geq R$$

$$\forall \; \mathbf{o} \in \mathcal{O}$$

where *R* is the radius of the circle. This constraint is added both for the different states *k* = 1 ... *N* as well as for all obstacles **o** ∈ O, which gives us in total of *N* × |O| where |O| is number of obstacles.

Circles that have have their centres at an offset (**c** = [*cx*, *cy*, *cR*]) also require that the platform orientation *θ* is considered. These obstacle constraints are formulated as:

$$\sqrt{\left(x\_k + c\_x \cos(\theta\_k) - c\_y \sin(\theta\_k) - o\_x\right)^2 + \left(y\_k + c\_x \sin(\theta\_k) + c\_y \cos(\theta\_k) - o\_y\right)^2} \ge c\_R \tag{24}$$

$$\forall \ \mathbf{o} \in \mathcal{O}, \forall \ \mathbf{c} \in \mathcal{C}$$

which gives us the total number of constraints added to be *N* × |O| × |C|, where |O| and |C| is the number of obstacles and circles respectively.

One difficulty not addressed up to this point is that each obstacle **o** is directly derived from sensory data and hence is subject to various sets of noise. If we are operating the robot close to an object it might happen that in one iteration the obstacles are on the "correct" side of the boundary, whereas in the next iteration they are not and the optimization problem is infeasible. To handle this situation we avoid constraints using the first state variable (*k* = 0) as this state is an initial condition and it is not possible to alter it in the optimization. In principle, one could also remove collision checks on further states ahead that then could handle larger noise values or cause the robot to back off when other dynamic entities such as people within the collision boundaries. As the trajectory is discretized based on *dt*, ignoring any step apart from *k* = 0 is not recommended especially if *dt* is large as not all actions effect the state and its collision boundaries will be incorporated and checked. If there is no feasible solution the vehicle is stopped; in practice this can happen if you quickly place an obstacle into the collision constraint zone, then the vehicle will simply stay still until this obstacle is removed.

Another approach to handling the noisy LiDAR data would be to incorporate the obstacle avoidance as part of the objective function. One option here would then be to utilize sigmoid functions that give a high cost if the the obstacle is within the collision boundary and a low one if it is not. One problem with this is that from an optimization point of view you also want an objective that is smooth (i.e., has a nice derivative) which from a numerical and practical point of view could be challenging. Due to this, we instead opted for using constraints on the obstacle avoidance and we found this approach to be more intuitive.

#### 5.4.3. Constraints to Avoid Singularities

As described in Section 4.3.3 there is a singular configuration when the ICR is very close to the wheel position which makes small changes in linear and angular velocities (**v**, *ω*) to create large changes in the steering angle of that wheel. To avoid control actions that are given in these singular regions, one option is to adopt the following set of constraints (one for each wheel position **p**1...4):

$$||\text{ICR} - \mathbf{p}\_i|| \le R \tag{25}$$

where *R* is the boundary radius that the ICR should not enter. However, while this constraints does seem to address the problem, the time complexity involved in adding these constraints was simply not worth it compared to using a more simplistic approach. Instead of adding this limitation into the optimization problem we can simply adjust the corresponding command sent to the platform (or have the platform do this internally) by adjusting the command **u** = (**v**, *ω*) to make sure that the above criteria ||ICR − **p**1...4|| holds by adjusting the control values; see Figure 6.

**Figure 6.** An example of the locations of ICR when moving around the platform, where the line represents the the consecutive changes of ICR locations. One can see the effect of *ω* (Equation (7)) that a relatively large change in ICR happens when there is low rotational velocity *ω*. In this example, we can see that the ICR was close to the boundary of wheel 4 (red) and probably was adjusted to stay outside the boundary. Note that to smoothly move from pure rotational velocity to linear velocity the ICR point has to incrementally move from the center passing in between the ICR constraint regions, therefore it is important to make the ICR constraint region not larger than needed.

This can be achieved quickly by first checking if the ICR is within any of the bounds. If not the command is valid and can be directly sent to the low level controller. If it is within any of the bounds we have the option of changing either the linear or angular velocity component or both before sending the command. Here we adopted the approach to adjust the linear **v** component and to keep the rotational velocity *ω*. One benefit of doing this at a higher level compared to just sending any control commands and letting the low-level controller handle it is that the the control values that will be used in the forward model will be more correct. It should also be noted that the optimization problem formulated here is to be used as a tracking-based controller as the key input is the next local goal pose, rather than a trajectory to follow, which makes the system robust to large disturbances.

Performing the navigation task though the doorway, as depicted in Figure 16 using ICR as constraints into the optimization had an average optimization time of 46 ± 29 ms compared to 29 ± 3 ms if ICR were instead corrected after optimization as described in the previous paragraph. It is worth noting the large variance which indicates that optimization problem is rather ill-posed but yet possible to solve.
