*Article* **A Local Planner for Accurate Positioning for a Multiple Steer-and-Drive Unit Vehicle Using Non-Linear Optimization**

**Henrik Andreasson 1,\*, Jonas Larsson <sup>2</sup> and Stephanie Lowry <sup>1</sup>**

<sup>2</sup> ABB Corporate Research, 722 26 Västerås, Sweden; jonas.larsson@se.abb.com

**\*** Correspondence: henrik.andreasson@oru.se

**Abstract:** This paper presents a local planning approach that is targeted for pseudo-omnidirectional vehicles: that is, vehicles that can drive sideways and rotate on the spot. This local planner—MSDU–is based on optimal control and formulates a non-linear optimization problem formulation that exploits the omni-motion capabilities of the vehicle to drive the vehicle to the goal in a smooth and efficient manner while avoiding obstacles and singularities. MSDU is designed for a real platform for mobile manipulation where one key function is the capability to drive in narrow and confined areas. The real-world evaluations show that MSDU planned paths that were smoother and more accurate than a comparable local path planner Timed Elastic Band (TEB), with a mean (translational, angular) error for MSDU of (0.0028 m, 0.0010 rad) compared to (0.0033 m, 0.0038 rad) for TEB. MSDU also generated paths that were consistently shorter than TEB, with a mean (translational, angular) distance traveled of (0.6026 m, 1.6130 rad) for MSDU compared to (0.7346 m, 3.7598 rad) for TEB.

**Keywords:** local planning; optimal control; obstacle avoidance

**1. Introduction**

Omni-motion capability—the ability to drive in all directions, not just forwards allows greater maneuverability for mobile robots and similar platforms [1,2]. A platform with restricted omni-motion capability must make more complex maneuvers to achieve goal positions [1], which can be difficult to plan [3], time-consuming, and even impossible in crowded or constrained physical environments [3]. Furthermore, greater mobile maneuverability allows a robot to achieve more accurate pose targets [4] which are essential for many industrial applications.

A fully holonomic platform has maximal omni-motion capability as such a platform can move freely in any direction [2], and fully holonomic motion can be achieved using omnidirectional wheels such as the Mecanum (or Swedish) wheel [5]. However, there are drawbacks to such a wheel configuration: these wheels are mechanically complex and the wheels are sensitive to non-smooth surfaces and thus require either clean, flat floors or complex suspension systems that ensure that the wheels are in contact with the ground [6]. For this reason, while fully holonomic systems are common in academic research [7], very few commercial platforms exist, although an exception is the Omnibot from KUKA [8].

Alternatively to a fully holonomic platform, a platform with a wheel configuration that uses multiple combined steer and drive wheels is an attractive option for an omni-motion robotic platform. It is not fully holonomic, but it has many advantages: it is robust to floor and environment conditions; it has good motion performance, with high acceleration, deacceleration, and turning capability; and it provides accurate wheel odometry, compared to the slip experienced by fully holonomic wheels.

However, a challenge with steerable wheels is the increased complexity in vehicle motion control. This is due to the resulting non-holonomic nature of the vehicle motion

**Citation:** Andreasson, H.; Larsson, J.; Lowry, S. A Local Planner for Accurate Positioning for a Multiple Steer-and-Drive Unit Vehicle Using Non-Linear Optimization. *Sensors* **2022**, *22*, 2588. https://doi.org/ 10.3390/s22072588

Academic Editor: Maysam Abbod

Received: 1 March 2022 Accepted: 24 March 2022 Published: 28 March 2022

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

<sup>1</sup> Centre for Applied Autonomous Sensor Systems (AASS), Örebro University, 701 82 Örebro, Sweden; stephanie.lowry@oru.se

as well as the inverse kinematics equations possessing singularities. Therefore, specialized motion-planning algorithms are required to ensure that the robot motion planning and control optimizes the agility and other advantages of the combined steer and drive wheels, while ensuring that the planner produces feasible paths for the platform to follow, and avoids singularities.

Existing local motion-planning algorithms often provide generic paths that do not fully exploit the pseudo-omnidirectional capabilities of the platform [9] or the kinodynamic constraints [10]. Furthermore, many local motion-planning algorithms do not inherently handle obstacles [11], which can be critical when navigating in confined and narrow environments. Furthermore, local motion planners often try to solve the global planning problem, which is computationally inefficient or infeasible for long paths [10]. Alternatively, local motion planners which do not solve the global path planning problem can cause the platform to get stuck in local minima and fail to reach the goal position [12,13].

The contribution of this paper is a local motion planner that exploits the pseudoomnidirectional capabilities of the platform to generate an efficient trajectory, allow precise positioning, and avoid singular configurations. It combines constraints from both vehicle dynamics and obstacles observed by the robot's perception module to ensure that the outputted trajectories are kinodynamically feasible as well as safe from obstacle collisions.

The local motion planner is designed to integrate with a global motion planner that provides an initial path estimate based on the robot's internal map of the environment. Coupling with a global planner avoids the need to address longer, time-consuming trajectory optimization steps or to use additional reasoning in the framework to select a shorter path [9], and ensures the platform avoids local minima. This global path estimate provides waypoints as sub-goals to the local planner. The local planner then computes the best local path and provides a control signal to the robot's low-level controller for navigation.

The local planner—denoted Multiple Steer Drive Unit Local Planner, or MSDU—is evaluated on a real-world robotic platform. It is demonstrated to provide more accurate positioning than existing local planners, achieving a smaller distance from the desired goal position in terms of both translation and angular displacement, while also taking a shorter path (in terms of both translational and angular distance) to the goal.

#### **2. Related Work**

This paper focuses on the problem of motion planning and trajectory optimization. The challenge of motion planning is to produce paths or trajectories for a robot to travel to a specified goal, where a path is a curvature defined as a continuous or discretized function and a trajectory also contains velocity information. Trajectory optimization is often difficult as obstacles and complex robot dynamics have to be considered. The motion planner needs to consider both kinematic and kinodynamic feasibility. A kinematically feasible output simply means that the output paths are possible to drive: for example, the paths have continuous curvature. A kinodynamically feasible output is one where the given velocity and acceleration bounds on the vehicle are met.

Local motion planners that handle short-term planning with a limited horizon are used in complex environments where obstacle avoidance is the key. One "classical" local planner is the Dynamic Window Approach (DWA) [14], which, given a goal and current sensory readings, searches for feasible velocities that will drive the robot towards the goal. Another approach that is commonly used in practice is based on the Elastic-Band (EB) planner [15]. However, these planners provides outputs that are not kinodynamically smooth and lack a velocity profile [10]. Other approaches based on potential fields have also been proposed [16].

The elastic band has been extended with an optimization framework TEB [9,17] and provides a local planner that utilize optimization, which provides a trajectory that is followed by a separate controller. TEB is designed for differential drive, car-like and full holonomic vehicles. In [9], a set of possible local trajectories were used blending the difference between global and local planning. TEB-based formulations have also been utilized for motion planning of manipulators [18].

There is a large variety of optimization-based approaches which, nowadays, are becoming more and more popular primarily thanks to the increased computational power available. Often the task of trajectory generation and tracking/control is separated into two different entities [19,20]. As trajectory optimization is essentially what is solved in the MPC scheme and for example, similar to the approach suggested here, Schoels et al. [10] is a mixture of these two as the proposed system both generates trajectories and performs the tracking. In general, a more complicated footprint in combination with limited dynamics of a platform makes both the global motion planning search part as well as the trajectory generation and tracking more difficult, hence the need to separate them.

As the output of a global planner is typically a path, we can also define approaches that generate trajectories to reach points along the route as "path following" approaches. A generic description of wheeled platforms and path following is described by Oftadeh et al. [11] which also contains evaluations of a combined steer and drive platform. One key difference here is how the controllers are coupled with the error directly between the current pose and corresponding static path and do not rely on horizon-based optimization, and the followed path is assumed to be free of obstacles. There are a large variety of path-following for cars, such as lateral control, which also is formulated for 4WS4WD (four-wheel-steering–four-wheel-driving) or 4WID (four-wheel-independent-drive) [21,22] and often focuses on more complex dynamics and stability at higher speeds and not on obstacle handling. Typically these car-like vehicles have a limitation on the amount of steering each wheel can undertake [23].

How to formulate obstacle handling efficiently into an optimization framework is of importance and is related to both how to represent the vehicle's footprint, but also how to represent the shape of the obstacles. Circular or ellipsoidal constraints is commonly used [24] but other techniques using, for example, convex polygons described through signed distance functions [10,24].

Depending on the kinematic configuration and footprint of the platform there are many different motion planners, such as RRT [25], probabilistic roadmaps [26] and lattice-based motion planners [19,27]. There are also many variants of the RRT planner, such as RRT\* [28] which is provably asymptotically optimal, RRT-Blossom [29] which is designed for highly constrained environments or RRT\*-UNF [30] which targets dynamic environments.

As the given platform has a rather simplistic footprint and good maneuverability, we exploit this to have a fast Dijkstra-based global planner that is fast to compute and therefore can be continuously re-invoked with an up-to-date representation containing dynamic and static obstacles.

#### **3. System Design**

This section introduces the system that the MSDU Local Planner is designed to integrate with, including the robotic platform, the available sensors, the existing planning and navigation capability and the low-level control functionality. The challenges associated with the pseudo-omnidirectional motion capabilities of the platform are presented in Section 4, which describes the geometry and kinematics of the combined steer and drive wheels, and how the velocity and control values can be suitably represented to provide constraints to the optimization formulation. Section 5 then introduces the MSDU Local Planner presented in this paper with Section 5.1 describing the formulation of the optimization problem.

#### *3.1. The Platform*

The robotic platform used in this work is presented in Figure 1. It has four steerable wheels located in each corner and is what is often termed as a "pseudo-omnidirectional" platform [16].

**Figure 1.** Robotic platform used in the evaluation. It has four combined steer and drive wheels located at each corner. The onboard computer is an Intel NUC i7.

The steerable wheels in the steerable wheel research platform are an in-house design and are based on the outrunner hub-motors design commonly used in commodified personal e-mobility vehicles. These motors are superior when it comes to the torque-to-cost ratio, which was utilized to realize a direct-drive (no gearbox) solution for both steering and driving motion. High-quality motion performance is obtained using high-accuracy position sensors and state-of-the-art motor control. The result is a compact, high-performance, reliable and very quiet actuation module.

The vehicle control is designed to receive independent linear velocities in the 2D plane and rotation (*vx*, *vy*, *ω*) . To ensure high-performance dynamics of the vehicle-motion reference tracking, a dynamic model is used to compute feedforward torque to joints in a prediction-closed loop correction scheme. A dedicated Kalman filter is employed to make the vehicle control tolerant to potentially relatively low frequency and jittery references coming from the motion planner in the transition to the high frequency real-time system. In the tests reported in the present paper, the vehicle control system is reading the communication bus for new references from the planner at 500 Hz.

The wheel odometry computation is designed to take advantage of the redundancy of this specific configuration of four steerable wheels such that it selects wheels to be included to compute the measured 2D pose based on the motion. For example, any wheel that is close to ICR is omitted from the odometry calculation. This results in an accurate and robust measured vehicle-speed estimation, which is communicated to the navigation system at 500 Hz.

#### *3.2. Steerable Wheels*

The wheel configuration of multiple combined steer and drive wheels units was selected as it can provide omni-motion capability, while also having other benefits relative to other comparable systems:


inherent redundancy that enables diagnostics of vehicle motion estimation based on wheel motions.


Note that having more than two combined steer and drive wheels will provide redundancy to the system that can be beneficial for better traction in general and better handling of non-flat surfaces.
