*7.2. Local Planning Capabilities*

To give an intuitive understanding of what types of problem the local planner can address, and to also give some illustration regarding how obstacles are extracted and how collision constraints are formed, we performed the following task. When the robot was located in a doorway, we performed a 180◦ turn on the spot. As the global planner does not have a notion of the shape of the robot, it just gave the goal location directly. However, due to the shape of the vehicle it could not turn on the spot without causing a collision between the robot and the door frame; instead it had to move out of the doorway and there perform the rotation before entering the doorway again. This is a problem that the

optimization-based local planner addressed out of the box and snapshots of this turn are depicted in Figure 15.

**Figure 15.** Set of snapshots showing the capability of the local planner to do a 180◦ turn inside a narrow doorway where there is not enough space to turn on the spot. Top left is the start configuration and bottom right is the goal configuration. In each picture, the red arrow depicts the current goal. The green arrows depict the future horizon states (*x*, *y*, *θ*)1...*N*. The blue circles depicts the collision circles **c**1...*C*. The yellow squares is the extracted obstacle points **o**1...*O*. The red smaller dots are the LiDAR points.

#### *7.3. Passing a Doorway*

To illustrate the connection between the global planner and the proposed local planner when passing through a doorway, a sequence of images are depicted in Figure 16. The local goal orientation is computed by the directions given by the consecutive points before and after the current local goal point.

**Figure 16.** Set of snapshots illustrating the interaction between the global planner (the planned path is depicted as a black line and the goal is light-blue). The red arrow depicts the current local goal that the proposed local planner is driving towards, the green arrows depict the future horizon states (also see the caption in Figure 15. Due to the look ahead, the optimization allows for a smoother driven path compared to the relatively non-smooth and jerky path provided by the global path planner. The additional spacing between the obstacle points and the robot when there is enough space can be seen in the top left figure as the cost utilized by the global planner (illustrated with different gray levels) contains an additional offset. By setting this offset to at least corresponding to a single collision radius of the platform, that the platform can be in any orientation makes the obstacle constraints only to be active in areas where there is simply not enough space. Additionally, this additionally safety margin is very useful as the robot would otherwise drive very close to obstacles as the obstacle constraints do not have any other cost associated with them based on distance.

#### *7.4. Summary of Results*

The experiments demonstrate that MSDU consistently generated paths for the vehicle that were more accurate than those planned by TEB. Over the long paths in the experimental evaluation, MSDU had a mean translational error of 0.0019 m while TEB had a mean translational error of 0.0031 m. The mean angular error for MSDU was 0.0007 rad compared to 0.0025 rad for TEB. Over the (more challenging) short paths, MSDU had a mean translational error of 0.0028 m while TEB had a mean translational error of 0.0033 m. The mean angular error for MSDU was 0.0010 rad compared to 0.0038 rad for TEB.

The MSDU paths were also shorter than the TEB paths. Over the long paths, MSDU had a mean translational distance of 3.0011 m while TEB had a mean translational distance of 3.1877 m. Over the short paths, MSDU had a mean translational distance of 0.6026 m while TEB had a mean translational distance of 0.7346 m. In terms of angular distance traveled, MSDU had a mean angular distance traveled of 1.1690 rad over the long paths and 1.6130 rad over the short paths, while TEB had a mean angular distance traveled of 3.8198 rad over the long paths and 3.7598 rad over the short paths. In the worst-case scenario the TEB path traversed an angular distance 51 times as long as that of MSDU.

#### **8. Conclusions**

This work presents a local planner for mobile autonomous platforms with combined steer and drive wheels. It uses a non-linear optimization formulation to handle the kinematic characteristics of the wheel configuration, the physical limitations of the platform, and the obstacles in the environment, to generate a trajectory that generates smooth and efficient paths to a goal, and achieves highly accurate goal pose positions.

The local planner is tightly integrated into a full localization, navigation and motionplanning pipeline, and utilizes a global planner that is continuously re-executed to give an up-to-date global feasible path with respect to the latest readings and map updates. This enables the local planner to look exclusively at local problems and provide an online control signal based on immediate sensor and motion data, while the global planner resolves problems that require a broader global knowledge of the environment.

An interesting future research direction is how dynamic obstacles in the environment such as people, or other autonomous robots—could be managed by the system. An interesting and non-trivial question to ask is in which layer of the system the management of dynamic obstacles should be included. For example, if we obtain a prior knowledge of typical movement in the environment then this would be suitable to use when providing the global plan. Dynamic obstacles can also be relatively straightforwardly formulated as constraints connected to the predicted horizon if we have a model of how these obstacles move in the future.

In the current formulation, the actual steering wheel direction is not explicitly modeled. It would be of interest to study how to incorporate these in an efficient way. If we can look at the steering angle and the rate of change directly as constraint many detours taken in the current approach, such as taking care of the ICR, would indirectly be handled. One problem is that with a set of four steer-and-drive wheels there is a significant amount of redundancy that is far from straightforward to put together as an optimization problem that both gives reasonable outputs and is also sufficiently fast to solve.

**Author Contributions:** The individual contributions of the authors are as follows. Conceptualization, H.A. and J.L.; methodology, validation, investigation, draft preparation and writing, H.A., S.L. and J.L.; software and formal analysis, H.A. and S.L.; supervision, project administration and funding acquisition, H.A. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research was funded by FORMAS (Swedish Research Council for Sustainable Development), grant number 2019-02264.

**Institutional Review Board Statement:** Non applicable.

**Informed Consent Statement:** Non applicable.

**Data Availability Statement:** Not applicable.

**Conflicts of Interest:** The authors declare no conflicts of interest.
