*3.3. Perception*

The platform is equipped with a SICK TiM571 2D LiDAR, which is used for localization, navigation, and planning, as well as for obstacle detection. It is also used for building the map (see Figure 8) used by the robot to perform localization, navigation, and planning. The sensor characteristics taken from the data sheet are a systematic errors of ±60 mm and statistical errors of ±20 mm at ranges up to max range of 25 m with 90% remission and for 10% remission up to 6 m range the statistical error is ±10 mm; however, all numbers are typical and dependent on ambient conditions.

#### *3.4. Localization, Navigation, and Planning System*

The platform is equipped with all systems required for navigation; localization, global planning and the proposed MSDU local planning (where MSDU stands for Multiple Steer-And-Drive Unit) that provides the control output. All system components exist as ROS [31] packages and the proposed local planner is compatible with the navigation stack [32]. When provided with a map of its environment, the robot can localize within its map, using its range sensor and odometry readings. As a localization module we utilize the AMCL package with only minor parametric changes; the motion model is set to "omni-corrected". The robot also has a global planner that can provide a path based on the robot's map of the environment and also incorporated LiDAR readings. The global planner we use is the planner provided in the navigation stack with default parameters. The output is a path containing a set of poses and does not contain any velocity information.

The platform footprint is encoded as two circles. One circle has an inscribed radius, which gives the smallest opening you could drive the robot through with a *specific* angle, and the second circle has a circumscribed radius which is the smallest opening you could drive the robot through given *any* angle. What this shows us is that the global planner does not fully factor in the shape of the platform and it is up to the local planner to handle narrow passages where the opening size is between the inscribed and circumscribed radii, as can be seen in Figure 16.

Furthermore, the global planner does not consider the full kinematics of the robot and the generated global path is non-smooth. However, the global planner does consider the surrounding using LiDAR data (through the local costmap) and the given map (global costmap) to rapidly compute paths that keeps a distance from obstacles and that find plans that are suitable from a global perspective.

The local planner presented in this paper is designed to integrate with the global planner and use the provided path to generate waypoints as input to the local planner, which then formulates a control output for the low-level wheel controller. Unlike the global planner, the local planner factors in a large number of constraints, including the kinematic constraints of the robot and a footprint that can handle different orientation (to ensure safe navigation through confined spaces). Furthermore, the local planner integrates real-time information from the LiDAR sensor to guarantee that the generated control actions provide motions that are collision free.

Figure 2 presents an overview of the different components of the system and how they interact. As can be seen, the local planner integrates information from many components, including the global planner and the sensor data from the LiDAR, to calculate and output a control signal to the robot.

**Figure 2.** Overview of the different components of the localization, navigation and planning system and their interactions.

#### **4. Geometry and Kinematics of the Combined Steer and Drive Wheels**

This section provides an overview of the geometry and kinematics of the combined steer and drive wheels present on the robotic platform used in this paper. It outlines the physical limitations on the linear and angular velocities of the wheels, which will be used to provide the constraints for the optimization formulation in Section 5 below. Please note that we do not consider nor model any wheel slippage.

#### *4.1. Definitions*

Let the input control signal to the platform be denoted **u***<sup>p</sup>* = (**v**, *ω*) where **v** = (*vx*, *vy*) is the linear velocity (a 2D vector) and *ω* is the rotational velocity (a scalar). Using a 2D world reference frame {*A*}, let *θ* denote the orientation of the platform and *x*, *y* its 2D position (see Figure 3a). Then we have the following equations for the velocity of the platform in the world frame:

$$\begin{aligned} \dot{x}^{\{A\}} &= v\_x^{\{A\}} \\ \dot{y}^{\{A\}} &= v\_y^{\{A\}} \\ \dot{\theta}^{\{A\}} &= \omega \end{aligned} \tag{1}$$

**Figure 3.** Platform pose and coordinate frames used. (**a**) The platform pose is denoted by position (*x*, *y*) and orientation *θ* in a world frame; (**b**) the relationship between the world coordinate frame {*A*} and the platform coordinate frame {*B*}.

We also have the platform frame {*B*}, which has its origin at the center of the platform. The {*B*} frame is defined so that the *x* direction will point "forward" (see Figure 3b). If the velocity is defined in the platform frame {*B*}, the resulting equations become:

$$\begin{aligned} \dot{\mathbf{x}}^{\{A\}} &= \boldsymbol{v}\_{\mathbf{x}}^{\{\overline{\beta}\}} \cos(\theta) - \boldsymbol{v}\_{\mathbf{y}}^{\{\overline{\beta}\}} \sin(\theta) \\ \dot{\mathbf{y}}^{\{A\}} &= \boldsymbol{v}\_{\mathbf{x}}^{\{\overline{\beta}\}} \sin(\theta) + \boldsymbol{v}\_{\mathbf{y}}^{\{\overline{\beta}\}} \cos(\theta) \\ \dot{\theta}^{\{A\}} &= \omega \end{aligned} \tag{2}$$

#### *4.2. Wheel Configuration*

On the platform there are four combined steer and drive wheels ( *W*1...4). We denote for each wheel *Wi* the velocity *vi* and the steering angle *ϕi*. The position of the wheel **p**{*B*} *<sup>i</sup>* = (*p<sup>i</sup> <sup>x</sup>*, *p<sup>i</sup> <sup>y</sup>*) is given in the platform frame. The velocity vector **v***<sup>i</sup>* is used to represent the combination of *ϕ<sup>i</sup>* and *vi* (see Figure 4).

**Figure 4.** Wheel parameters: **p**<sup>1</sup> indicates the position, **v**<sup>1</sup> the velocity vector, *vi* = **v***<sup>i</sup>* the velocity (scalar) and *ϕ*<sup>1</sup> is the steering (scalar).

#### *4.3. The Effect of the Control Input on the Wheels*

Given a control value **u** = (**v**, *ω*) to the platform, it is necessary is to compute the corresponding steering angle *ϕ<sup>i</sup>* and drive wheel velocity *vi* of each wheel.

We therefore seek a function *f* with the following property:

$$f(\boldsymbol{\varrho}\_{i\prime}\boldsymbol{v}\_{i}) = f(\mathbf{p}\_{i}^{\{\boldsymbol{B}\}}, \mathbf{v}^{\{\boldsymbol{B}\}}, \boldsymbol{\omega}).\tag{3}$$

In this formulation, **v** (and **p**) are expressed in the platform frame {*B*}.

Intuitively, function *f* consists of two different components. (1) *f***<sup>v</sup>** which handles linear motion and (2) *fω* which takes care of rotational motion. The function is a combination of the two: " *f* = *f***<sup>v</sup>** + *fω*".

4.3.1. Linear Velocity Component *f***<sup>v</sup>**

Firstly, assume there is only a linear control component **v** (and *ω* = 0). This would correspond to each wheel having the same steering angle (*ϕ*<sup>1</sup> = *ϕ*<sup>2</sup> = *ϕ*<sup>3</sup> = *ϕ*4), given by the velocity vector **v** as:

$$\begin{aligned} (\varphi\_{i\prime} v\_{i\prime}) &= f(\mathbf{p}\_{i\prime} \mathbf{v}\_{\prime} 0) \\ \varphi\_{i} &= \operatorname\*{atan2}(v\_{\mathcal{Y}\prime} v\_{\mathcal{X}}) \\ v\_{i} &= ||\mathbf{v}|| \end{aligned} \tag{4}$$

Note that: (1) the steering angle is independent on the wheels' position **p**1...4, and (2) when there is no velocity **v** given in this case, the steering angles *ϕ*1...4 can be arbitrarily set.

4.3.2. Rotational Velocity Component—*fω*

Now, assume that there is only a rotational velocity control component *ω* (and **v** = **0**). Given that *ω* = 0 the steering angles can be computed as (see also Figure 5):

$$\begin{aligned} (\varphi\_{i\prime} v\_i) &= f(\mathbf{p}\_i, \mathbf{0}, \omega) \\ \varphi\_i &= \operatorname{atan2}(p\_{y\prime}^i p\_x^i) + \frac{\pi}{2} \\ \mathbf{v}\_i &= \omega \times \mathbf{p}\_i \\ v\_i &= ||\mathbf{v}\_i|| \end{aligned} \tag{5}$$

where for the × computation we have assumed that *ω* = [0 0 *ω*] *<sup>T</sup>* and **p***<sup>i</sup>* = [*p<sup>i</sup> <sup>x</sup> p<sup>i</sup> <sup>y</sup>* 0] *T*. Note that **v***<sup>i</sup>* is a vector (compared to *vi* which only is a scalar).

**Figure 5.** Wheel 1 parameters given that **v** = **0**, which enforces the steering wheel angle *ϕ*<sup>1</sup> to make the steering direction vector **v**<sup>1</sup> to be orthogonal towards the origin of the frame.

Note that if there is a wheel located at the center of the platform frame (*p<sup>i</sup> <sup>x</sup>* = *p<sup>i</sup> <sup>y</sup>* = 0), this wheel angle is not defined and could in principle be set arbitrarily; note that this point is the Instantaneous Center of Rotation (ICR).

#### 4.3.3. Combining Linear and Rotational Components

From the previous sections we have obtained a set of linear velocity vectors (**v** and **v**1...4), where **v** originates from the linear velocity component *f***<sup>v</sup>** and **v**1...4 from *fω*.

These velocity vectors are combined through vector summation as follows:

$$\begin{aligned} (\boldsymbol{\varrho}\_{i\prime} \boldsymbol{v}\_{i}) &= f(\mathbf{p}\_{i\prime} \mathbf{v}\_{\prime} \boldsymbol{\omega}) \\ \boldsymbol{\varrho}\_{i} &= \underbrace{\boldsymbol{\mu} \times \mathbf{p}\_{i} + \mathbf{v}}\_{\parallel \boldsymbol{\omega} \times \mathbf{p}\_{i} + \mathbf{v}} \\ \boldsymbol{v}\_{i} &= ||\boldsymbol{\omega} \times \mathbf{p}\_{i} + \mathbf{v}|| \end{aligned} \tag{6}$$

Note that if **v** = −*ω* × **p***i*, we have the point of rotation (ICR) at **p***i*. Note at this exact point the wheel can in principle be set arbitrarily, however, this point is singular as small changes to the ICR will require rapid changes to the angle *ϕ*. In short, this singular point can be avoided by ensuring that the ICR point is sufficiently distant from **p***i*.

The ICR point is a function of the control **u***<sup>p</sup>* = (**v**, *ω*)=(*vx*, *vy*, *ω*) as:

$$\begin{aligned} \text{ICR}\_{\text{x}} &= -\frac{v\_{\text{y}}}{\omega} \\ \text{ICR}\_{\text{y}} &= \frac{v\_{\text{x}}}{\omega} \end{aligned} \tag{7}$$

Note that if there is no rotational element *ω*, the ICR will be at infinity.
