<figure><img src="image_1.png" /></figure>

HAL
open science

# New Navigation Algorithms for Autonomous Mobile Robots in Intra-logistics Operations

Hussein Harb

<figure><img src="image_2.png" /></figure>

## To cite this version:

Hussein Harb. New Navigation Algorithms for Autonomous Mobile Robots in Intra-logistics Operations. Université Grenoble Alpes. 2023, pp.32. hal-04616509

HAL Id: hal-04616509

https://hal.science/hal-04616509

Submitted on 18 Jun 2024

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

Public Domain
<figure><img src="image_1.png" /></figure>

HAL
open science

# New Navigation Algorithms for Autonomous Mobile Robots in Intra-logistics Operations

Hussein Harb

<figure><img src="image_2.png" /></figure>

## To cite this version:

Hussein Harb. New Navigation Algorithms for Autonomous Mobile Robots in Intra-logistics Operations. Université Grenoble Alpes. 2023, pp.32. hal-04616509

HAL Id: hal-04616509

https://hal.science/hal-04616509v1

Submitted on 18 Jun 2024

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

Public Domain
<figure><img src="image_3.png" /></figure>

UNIVERSITÉ
Grenoble
Alpes

<figure><img src="image_4.png" /></figure>

Master TSI - Mobile, Autonomous and Robotic Systems (MARS)

# New Navigation Algorithms for Autonomous Mobile Robots in Intra-logistics Operations

Hussein Harb

29/07/2023

Research project performed at G-SCOP and INRIA

Under the supervision of:
Pierre DAVID
Thierry FRAICHARD

Defended before a jury composed of:
Ahmad Hably

June 2023
<figure><img src="image_3.png" /></figure>

UNIVERSITÉ
Grenoble
Alpes

<figure><img src="image_4.png" /></figure>

Master TSI - Mobile, Autonomous and Robotic Systems (MARS)

# New Navigation Algorithms for Autonomous Mobile Robots in Intra-logistics Operations

Hussein Harb

29/07/2023

Research project performed at G-SCOP and INRIA

Under the supervision of:
Pierre DAVID
Thierry FRAICHARD

Defended before a jury composed of:
Ahmad Hably

June 2023
# Contents

<table><tr><td>1</td><td>Introduction</td><td>1</td></tr><tr><td>2</td><td>Problem Description</td><td>2</td></tr><tr><td>2.1</td><td>Mission</td><td>2</td></tr><tr><td>2.2</td><td>Workspace</td><td>3</td></tr><tr><td>3</td><td>Navigation Constraints</td><td>4</td></tr><tr><td>3.1</td><td>Logistics</td><td>4</td></tr><tr><td>3.2</td><td>Social</td><td>5</td></tr><tr><td>3.2.1</td><td>Personal Space</td><td>5</td></tr><tr><td>3.2.2</td><td>Information Process Space</td><td>5</td></tr><tr><td>3.2.3</td><td>Interaction Space</td><td>5</td></tr><tr><td>3.2.4</td><td>Activity and Affordance Space</td><td>6</td></tr><tr><td>3.3</td><td>Ergonomic</td><td>6</td></tr><tr><td>3.3.1</td><td>Comfort Speed</td><td>7</td></tr><tr><td>3.3.2</td><td>Approach Method</td><td>7</td></tr><tr><td>3.3.3</td><td>Acceleration</td><td>7</td></tr><tr><td>3.3.4</td><td>Passing Region</td><td>7</td></tr><tr><td>4</td><td>State-of-the-Art Of Material Handling Operations</td><td>8</td></tr><tr><td>4.1</td><td>Transfer Operation</td><td>8</td></tr><tr><td>4.1.1</td><td>Human Consideration</td><td>8</td></tr><tr><td>4.1.2</td><td>Transfer Motion Planners</td><td>8</td></tr><tr><td>4.2</td><td>Handover Operation</td><td>8</td></tr><tr><td>4.2.1</td><td>Handover Type</td><td>9</td></tr><tr><td>4.2.2</td><td>Type Of the Robot</td><td>9</td></tr><tr><td>4.2.3</td><td>Handover Motion Planners</td><td>9</td></tr><tr><td>5</td><td>Handover Problem Specification</td><td>10</td></tr><tr><td>5.1</td><td>Collision Avoidance</td><td>11</td></tr><tr><td>5.2</td><td>Speed</td><td>11</td></tr><tr><td>5.3</td><td>Personal Space</td><td>11</td></tr><tr><td>5.4</td><td>Field of View</td><td>12</td></tr><tr><td>5.5</td><td>Passing Region</td><td>12</td></tr><tr><td>5.5.1</td><td>Human body References</td><td>12</td></tr><tr><td>5.5.2</td><td>Passing Region</td><td>13</td></tr><tr><td>6</td><td>Material Handling Navigation Approach</td><td>14</td></tr><tr><td>6.1</td><td>Search Based State Lattice Planner</td><td>14</td></tr><tr><td>6.1.1</td><td>Search Based Planning</td><td>15</td></tr><tr><td>6.1.2</td><td>Discretization of State Space</td><td>15</td></tr><tr><td>6.1.3</td><td>State Validity and Collision Checking</td><td>16</td></tr><tr><td>6.1.4</td><td>Search Based Algorithm</td><td>16</td></tr><tr><td>6.1.5</td><td>Kinematic Constraints and Motion Primitives</td><td>17</td></tr></table>
# Contents

<table><tr><td>1</td><td>Introduction</td><td>1</td></tr><tr><td>2</td><td>Problem Description</td><td>2</td></tr><tr><td>2.1</td><td>Mission</td><td>2</td></tr><tr><td>2.2</td><td>Workspace</td><td>3</td></tr><tr><td>3</td><td>Navigation Constraints</td><td>4</td></tr><tr><td>3.1</td><td>Logistics</td><td>4</td></tr><tr><td>3.2</td><td>Social</td><td>5</td></tr><tr><td>3.2.1</td><td>Personal Space</td><td>5</td></tr><tr><td>3.2.2</td><td>Information Process Space</td><td>5</td></tr><tr><td>3.2.3</td><td>Interaction Space</td><td>5</td></tr><tr><td>3.2.4</td><td>Activity and Affordance Space</td><td>6</td></tr><tr><td>3.3</td><td>Ergonomic</td><td>6</td></tr><tr><td>3.3.1</td><td>Comfort Speed</td><td>7</td></tr><tr><td>3.3.2</td><td>Approach Method</td><td>7</td></tr><tr><td>3.3.3</td><td>Acceleration</td><td>7</td></tr><tr><td>3.3.4</td><td>Passing Region</td><td>7</td></tr><tr><td>4</td><td>State-of-the-Art Of Material Handling Operations</td><td>8</td></tr><tr><td>4.1</td><td>Transfer Operation</td><td>8</td></tr><tr><td>4.1.1</td><td>Human Consideration</td><td>8</td></tr><tr><td>4.1.2</td><td>Transfer Motion Planners</td><td>8</td></tr><tr><td>4.2</td><td>Handover Operation</td><td>8</td></tr><tr><td>4.2.1</td><td>Handover Type</td><td>9</td></tr><tr><td>4.2.2</td><td>Type Of the Robot</td><td>9</td></tr><tr><td>4.2.3</td><td>Handover Motion Planners</td><td>9</td></tr><tr><td>5</td><td>Handover Problem Specification</td><td>10</td></tr><tr><td>5.1</td><td>Collision Avoidance</td><td>11</td></tr><tr><td>5.2</td><td>Speed</td><td>11</td></tr><tr><td>5.3</td><td>Personal Space</td><td>11</td></tr><tr><td>5.4</td><td>Field of View</td><td>12</td></tr><tr><td>5.5</td><td>Passing Region</td><td>12</td></tr><tr><td>5.5.1</td><td>Human body References</td><td>12</td></tr><tr><td>5.5.2</td><td>Passing Region</td><td>13</td></tr><tr><td>6</td><td>Material Handling Navigation Approach</td><td>14</td></tr><tr><td>6.1</td><td>Search Based State Lattice Planner</td><td>14</td></tr><tr><td>6.1.1</td><td>Search Based Planning</td><td>15</td></tr><tr><td>6.1.2</td><td>Discretization of State Space</td><td>15</td></tr><tr><td>6.1.3</td><td>State Validity and Collision Checking</td><td>16</td></tr><tr><td>6.1.4</td><td>Search Based Algorithm</td><td>16</td></tr><tr><td>6.1.5</td><td>Kinematic Constraints and Motion Primitives</td><td>17</td></tr></table>
<table><tbody><tr><td>6.2</td><td>Model Predictive Control</td><td>17</td></tr><tr><td>7</td><td>Simulation and Results</td><td>18</td></tr><tr><td>7.1</td><td>Simulation Setup</td><td>18</td></tr><tr><td>7.1.1</td><td>Loading Scenario</td><td>18</td></tr><tr><td>7.1.2</td><td>Forward Passing Scenario</td><td>19</td></tr><tr><td>7.1.3</td><td>Cross Passing Scenario</td><td>19</td></tr><tr><td>7.2</td><td>Conclusion</td><td>20</td></tr><tr><td>A</td><td>Appendix</td><td>21</td></tr><tr><td>A.1</td><td>Local Planner</td><td>21</td></tr><tr><td>A.1.1</td><td>Model Predictive Controller</td><td>21</td></tr><tr><td>A.1.2</td><td>Motion Planning Problem Description</td><td>21</td></tr><tr><td>A.1.3</td><td>Local Planning via Optimal Control</td><td>21</td></tr><tr><td>A.1.4</td><td>Feedback Control</td><td>21</td></tr><tr><td>A.1.5</td><td>Numerical Realization</td><td>22</td></tr><tr><td></td><td>Alternative State Spaces</td><td>22</td></tr><tr><td></td><td>Direct Transcription</td><td>22</td></tr><tr><td></td><td>Receding-Horizon Quadratic-Form MPC</td><td>23</td></tr><tr><td></td><td>Obstacle Avoidance Constraints</td><td>23</td></tr><tr><td></td><td>Solution of the Nonlinear Program</td><td>24</td></tr><tr><td>Bibliography</td><td></td><td>25</td></tr></tbody></table>
<table><tr><td>6.2</td><td>Model Predictive Control</td><td>17</td></tr><tr><td>7</td><td>Simulation and Results</td><td>18</td></tr><tr><td>7.1</td><td>Simulation Setup</td><td>18</td></tr><tr><td>7.1.1</td><td>Loading Scenario</td><td>18</td></tr><tr><td>7.1.2</td><td>Forward Passing Scenario</td><td>19</td></tr><tr><td>7.1.3</td><td>Cross Passing Scenario</td><td>19</td></tr><tr><td>7.2</td><td>Conclusion</td><td>20</td></tr><tr><td>A</td><td>Appendix</td><td>21</td></tr><tr><td>A.1</td><td>Local Planner</td><td>21</td></tr><tr><td>A.1.1</td><td>Model Predictive Controller</td><td>21</td></tr><tr><td>A.1.2</td><td>Motion Planning Problem Description</td><td>21</td></tr><tr><td>A.1.3</td><td>Local Planning via Optimal Control</td><td>21</td></tr><tr><td>A.1.4</td><td>Feedback Control</td><td>21</td></tr><tr><td>A.1.5</td><td>Numerical Realization</td><td>22</td></tr><tr><td></td><td>Alternative State Spaces</td><td>22</td></tr><tr><td></td><td>Direct Transcription</td><td>22</td></tr><tr><td></td><td>Receding-Horizon Quadratic-Form MPC</td><td>23</td></tr><tr><td></td><td>Obstacle Avoidance Constraints</td><td>23</td></tr><tr><td></td><td>Solution of the Nonlinear Program</td><td>24</td></tr><tr><td>Bibliography</td><td></td><td>25</td></tr></table>
<figure><img src="image_5.png" /></figure>
<figure><img src="image_5.png" /></figure>
Introduction

As technology improves, the use of robots is expanding to various domains and because of the robotic systems'
efficiency, robustness, and precision, nowadays they are being massively used in the industry[50]. They are being
introduced into the industry for the aim increasing efficiency and reducing cost by automating various opera-
tions. One key operation that helps fulfill these requirements in the industry is material handling. It can be defined
as the movement, storage, protection and control of materials throughout the manufacturing and distribution pro-
cess, including their consumption and disposal. Material handling operations can include activities such as loading
and unloading of trucks and other transport vehicles. Movement of materials from receiving areas to storage lo-
cations, handling and disposal of waste materials[52]. Automating material handling operations is essential for
improving efficiency, reducing costs, and ensuring worker safety. It can be accomplished manually by a human
operator or automatically by using a conveyor belt, autonomous mobile robots (AMRs) or automated guided vehi-
cles (AGVs). Effective material handling requires careful planning, design, and implementation of processes and
equipment to minimize handling time, reduce damage to materials, and optimize use of space and resources. This
process can be a challenging task for operators, but the use of appropriate material handling equipment (MHE)
such as mobile robots can help to solve this problem[58]. Using automatic material handling equipment such as
conveyor belts requires preparations in the plant before use and it is difficult to modify upon need. In the other
hand, AMRs and AGVs can be easily implemented in any existing plant. Despite their similarities both has some
differences. The most fundamental difference between them is that AMRs are self-contained, highly flexible and
scalable as they do not require any wires, magnetic stripes, or other costly modifications to the building infrastruc-
ture for navigation, while AGVs do require such infrastructure for guidance [8] [44].

Working spaces for humans and machines overlaps upon the use of autonomous robots. This implies that a
new set of ergonomic constraints are set for the robot navigation strategy. Ergonomics is the science of designing
equipment, systems, and workplaces to maximize efficiency, productivity, and safety while minimizing discomfort
and injury.[60]. Mobile robots, like any other technological tool, should be designed with ergonomics in mind.

In material handling operation, AMR should retain some critical information such as due date and type of
loading and abide with some constraints to fulfill logistic requirements. Similarly, given that the robot is navigating
in human populated environment the robot should also abide by social constraints such as respecting the existence
of personal spaces, being aware about activity and affordance spaces and respecting the information process space.

Our aim is to develop a novel navigation strategy for AMRs in industrial setups that takes into account the
previously briefed constraints. The proposal aims to address developing a navigation algorithm for AMRs that
insures the accomplishment of a loading operation in a shared environment. To do so an extensive research about
the state of the art should be done and a proper definition of the problem should be considered. This definition
is made by searching for relevant work done and related to intralogistics in the sectors of logistics, social science
and navigation. Doing that all relevant constraints will be properly defined and that will allow us to chose a proper
navigation strategy that we can test, evaluate and deduce some preliminary results that will aid us in the process
of testing on real life scenarios.
# Introduction

As technology improves, the use of robots is expanding to various domains and because of the robotic systems' efficiency, robustness, and precision, nowadays they are being massively used in the industry[50]. They are being introduced into the industry for the aim increasing efficiency and reducing cost by automating various operations. One key operation that helps fulfill these requirements in the industry is material handling. It can be defined as the movement, storage, protection and control of materials throughout the manufacturing and distribution process, including their consumption and disposal. Material handling operations can include activities such as loading and unloading of trucks and other transport vehicles, Movement of materials from receiving areas to storage locations, handling and disposal of waste materials[52]. Automating material handling operations is essential for improving efficiency, reducing costs, and ensuring worker safety. It can be accomplished manually by a human operator or automatically by using a conveyor belt, autonomous mobile robots (AMRs) or automated guided vehicles (AGVs). Effective material handling requires careful planning, design, and implementation of processes and equipment to minimize handling time, reduce damage to materials, and optimize use of space and resources. This process can be a challenging task for operators, but the use of appropriate material handling equipment (MHE) such as mobile robots can help to solve this problem[58]. Using automatic material handling equipment such as conveyor belts requires preparations in the plant before use and it is difficult to modify upon need. In the other hand, AMRs and AGVs can be easily implemented in any existing plant. Despite their similarities both have some differences. The most fundamental difference between them is that AMRs are self-contained, highly flexible and scalable as they do not require any wires, magnetic stripes, or other costly modifications to the building infrastructure for navigation, while AGVs do require such infrastructure for guidance [8] [44].

Working spaces for humans and machines overlaps upon the use of autonomous robots. This implies that a new set of ergonomic constraints are set for the robot navigation strategy. Ergonomics is the science of designing equipment, systems, and workplaces to maximize efficiency, productivity, and safety while minimizing discomfort and injury.[60]. Mobile robots, like any other technological tool, should be designed with ergonomics in mind.

In material handling operation, AMR should retain some critical information such as due date and type of loading and abide with some constraints to fulfill logistic requirements. Similarly, given that the robot is navigating in human populated environment the robot should also abide by social constraints such as respecting the existence of personal spaces, being aware about activity and affordance spaces and respecting the information process space.

Our aim is to develop a novel navigation strategy for AMRs in industrial setups that takes into account the previously briefed constraints. The proposal aims to address developing a navigation algorithm for AMRs that insures the accomplishment of a loading operation in a shared environment. To do so an extensive research about the state of the art should be done and a proper definition of the problem should be considered. This definition is made by searching for relevant work done and related to intralogistics in the sectors of logistics, social science and navigation. Doing that all relevant constraints will be properly defined and that will allow us to chose a proper navigation strategy that we can test, evaluate and deduce some preliminary results that will aid us in the process of testing on real life scenarios.
# Problem Description

In this section we are going to present a general description of the problem tackled by specifying the assigned mission and the operation environment. Mobile robots are used for various tasks such as transporting goods to increase the tasks' efficiency [17]. Upon transporting the good a mobile robot should be assigned a clear mission and fully aware about the environment layout. These two factors describe in general the basic constraints for a mobile robot in industry.

## 2.1 Mission

To begin with, in general a simple transportation of goods mission of a mobile robot is composed of two main procedures: transfer and loading-unloading. Transfer operation is defined as the process of navigating through a defined environment abiding by different constraints such as avoiding obstacles and reaching a defined end goal from a defined start position.

<figure><img src="image_6.png" /><figcaption>Figure 2.1: Robot Transfer Operation [47]</figcaption></figure>

On the other hand, loading and unloading is the process of navigating in a certain area for the aim of delivering or receiving certain goods. This process takes place either at the start point or at the goal point as shown in figure 2.1. During this process, handling goods can be automatically done by machines or conveyor belts as shown in figure 2.2a or manually by a human operator 2.2b. A different set of constraints is defined for each loading, unloading, and transfer procedure. In our work we will focus mainly on the loading-unloading part of the mission.

<figure><img src="image_7.png" /><figcaption>Figure 2.2: Loading and unloading Processes</figcaption></figure>

<figure><img src="image_8.png" /></figure>
# Problem Description

In this section we are going to present a general description of the problem tackled by specifying the assigned mission and the operation environment. Mobile robots are used for various tasks such as transporting goods to increase the tasks' efficiency [17]. Upon transporting the good a mobile robot should be assigned a clear mission and fully aware about the environment layout. These two factors describe in general the basic constraints for a mobile robot in industry.

## 2.1 Mission

To begin with, in general a simple transportation of goods mission of a mobile robot is composed of two main procedures: transfer and loading-unloading. Transfer operation is defined as the process of navigating through a defined environment abiding by different constraints such as avoiding obstacles and reaching a defined end goal from a defined start position.

<figure><img src="image_6.png" /><figcaption>Figure 2.1: Robot Transfer Operation [47]</figcaption></figure>

On the other hand, loading and unloading is the process of navigating in a certain area for the aim of delivering or receiving certain goods. This process takes place either at the start point or at the goal point as shown in figure 2.1. During this process, handling goods can be automatically done by machines or conveyor belts as shown in figure 2.2a or manually by a human operator 2.2b. A different set of constraints is defined for each loading, unloading, and transfer procedure. In our work we will focus mainly on the loading-unloading part of the mission.

<figure><img src="image_7.png" /><figcaption>Figure 2.2: Loading and unloading Processes</figcaption></figure>

<figure><img src="image_8.png" /></figure>
## 2.2 Workspace

Secondly, a mobile robot should be aware about the constraints projected from the environment. These constraints can be defined as areas in the factory layout. These constraints are projected from the environment and can be illustrated by the following example of a typical plant layout shown in figure 2.3a. A typical plant layout consists of the following different generalized areas shown in figure 2.3a. First category employee facilities. These areas include break-rooms, restrooms, locker rooms, offices, first-aid stations, parking, safety equipment areas and exercise areas. Second, is the work stations which includes workbenches, production lines, different types of machines, and seating. Third is the storage areas which includes racks, shelving, pallets, storage containers, charging stations. Finally, fourth is the receiving and departure areas which includes loading-unloading docks, dock leveler, storage containers, pallet jacks, forklift, weighting scales, and packaging supplies.

The different types of elements found in the warehouse environment are divided into architectural elements, mobile objects, and stationary objects. Architectural elements are the elements built in the environment and cannot be moved. Stationary objects are the objects fixed in place and cannot be easily moved or relocated. Mobile objects are not stationary and can be easily moved from one place to another. Table 2.1 below shows examples of these elements.

<table><caption>Table 2.1: Examples of Elements In a Warehouse</caption><tr><td>Architectural Elements</td><td>doors, walls, stairs, lifts, support columns</td></tr><tr><td>Stationary Objects</td><td>workbenches, product lines, different types of machines, shelves, offices, charging stations, weight scales</td></tr><tr><td>Mobile objects</td><td>chairs, pallets, containers, boxes, dock leveler, forklifts</td></tr></table>

The navigation robot will handle this layout as a 2D workspace structure that handles traffic rules. Figure 2.3b illustrates the different types of spaces inside this environment. These areas consist of forbidden areas which are the areas that the robot cannot navigate in. These areas may include places where different obstacles exist such as architectural elements, stationary object and mobile objects or areas that are defined by the plant to be forbidden areas for the robot to navigate in such as rooms with high magnetic field and employee facilities. These areas are marked in black in figure 2.3b. Next, are the allowed areas. These areas are the areas the robot is allowed to navigate in. Allowable areas include critical areas, loading-unloading areas and travel lanes. The critical areas are the areas that the robot can pass through but cannot stop in and has to abide by a certain speed range. For example, when the robot navigates through an existing door it should be aware that a human can suddenly appear in that area and it should be ready to avoid him. Moreover the robot should be aware that in these areas it should not stop and block the passage. Following, are the loading and unloading areas. The loading and unloading areas in a plant are designated spaces where goods, equipment, and materials are loaded onto and unloaded from mobile robot. These areas have their own set of constraints since in manual case collaboration between the mobile robot and the human is essential for the completion of the process. Figure 2.3b also introduces the travel lanes. They are divided in human travel lanes and robot travel lanes. Similarly human travel lanes are the lanes dedicated for human traveling in this area. Finally, the robot might need to communicate with an object to complete the mission. For instance, having a closed door in the environment will require the robot to communicate with it for a request to open the closed door to pass. This is denoted as robot interactable objects in the figure 2.3b. Moreover in complex environment such as the one present in the factory where humans, robots and machines collaborate to perform a specific task, another constraints should be taken into consideration such as logistics, social and ergonomics which will be emphasized more in the navigation constraints section.

<figure><img src="image_9.png" /><figcaption>(a) Typical Plant Layout</figcaption></figure>

<figure><img src="image_10.png" /><figcaption>(b) 2D Workspace Structure and Traffic Rule</figcaption></figure>

<caption>Figure 2.3: Typical Plant Layout and Structure</caption>
## 2.2 Workspace

Secondly, a mobile robot should be aware about the constraints projected from the environment. These constraints can be defined as areas in the factory layout. These constraints are projected from the environment and can be illustrated by the following example of a typical plant layout shown in figure 2.3a. A typical plant layout consists of the following different generalized areas shown in figure 2.3a. First category employee facilities. These areas are break-rooms, restrooms, locker rooms, offices, first-aid stations, parking, safety equipment areas and exercise areas. Second, is the work stations which includes workbenches, production lines, different types of machines, and seating. Third is the storage areas which includes racks, shelving, pallets, storage containers, charging stations. Finally, fourth is the receiving and departure areas which includes loading-unloading docks, dock leveler, storage containers, pallet jacks, forklift, weighting scales, and packaging supplies.

The different types of elements found in the warehouse environment are divided into architectural elements, mobile objects, and stationary objects. Architectural elements are the elements built in the environment and cannot be moved. Stationary objects are the objects fixed in place and cannot be easily moved or relocated. Mobile objects are not stationary and can be easily moved from one place to another. Table 2.1 below shows examples of these elements.

<table><caption>Table 2.1: Examples of Elements In a Warehouse</caption><tr><td>Architectural Elements</td><td>doors, walls, stairs, lifts, support columns</td></tr><tr><td>Stationary Objects</td><td>workbenches, product lines, different types of machines, shelves, offices, charging stations, weight scales</td></tr><tr><td>Mobile objects</td><td>chairs, pallets, containers, boxes, dock leveler, forklifts</td></tr></table>

The navigation robot will handle this layout as a 2D workspace structure that handles traffic rules. Figure 2.3b illustrates the different types of spaces inside this environment. These areas consist of forbidden areas which are the areas that the robot cannot navigate in. These areas may include places where different obstacles exist such as architectural elements, stationary object and mobile objects or areas that are defined by the plant to be forbidden areas for the robot to navigate in such as rooms with high magnetic field and employee facilities. These areas are marked in black in figure 2.3b. Next, are the allowed areas. These areas are the areas the robot is allowed to navigate in. Allowable areas include critical areas, loading-unloading areas and travel lanes. The critical areas are the areas that the robot can pass through but cannot stop in and has to abide by a certain speed range. For example, when the robot navigates through an existing door it should be aware that a human can suddenly appear in that area and it should be ready to avoid him. Moreover the robot should be aware that in these areas it should not stop and block the passage. Following, are the loading and unloading areas. The loading and unloading areas in a plant are designated spaces where goods, equipment, and materials are loaded onto and unloaded from mobile robot. These areas have their own set of constraints since in manual case collaboration between the mobile robot and the human is essential for the completion of the process. Figure 2.3b also introduces the travel lanes. They are divided in human travel lanes and robot travel lanes. Similarly human travel lanes are the lanes dedicated for human traveling in this area. Finally, the robot might need to communicate with an object to complete the mission. For instance, having a closed door in the environment will require the robot to communicate with it for a request to open the closed door to pass. This is denoted as robot interactable objects in the figure 2.3b. Moreover in complex environment such as the one present in the factory where humans, robots and machines collaborate to perform a specific task, another constraints should be taken into consideration such as logistics, social and ergonomics which will be emphasized more in the navigation constraints section.

<figure><img src="image_9.png" /></figure>

<figure><img src="image_10.png" /><figcaption>Figure 2.3: Typical Plant Layout and Structure</figcaption></figure>
# Navigation Constraints

In this chapter we are going to explore the different constraints for AMRs in intralogistics operations. It covers various factors coming from logistics sector such as precise identification of loading and delivery points, types of loads and deliveries, time constraints, load features, interaction with operators, desired speed. Moreover social and ergonomic constraints are considered such as adhering to social rules, and promoting safe and comfortable human-robot interaction. Due to the limited time of the internship we mentioned all relevant constraints but not all of them are implemented in our approach. This serves the proper understanding and formalization of the problem. In the approach section implemented constraints are pointed out.

## 3.1 Logistics

To ensure efficient and effective operation, an AMR should comply with several constraints projected from the logistic nature of the operation. Firstly, the location of the loading station, which is the point where the robot will pick up the load, should be precisely identified and programmed into the robot's system. This is important to ensure that the robot goes to the right location to pick up the load and minimize the time taken for the operation.

The delivery point, which is the location where the robot will drop off the load, should also be identified and programmed into the robot's system. This information helps the robot to navigate to the correct location, and it also helps in minimizing the time taken for the delivery.

After identifying the loading-unloading position the type of load and delivery should be defined. The type of load carried by an AMR has a significant impact on the robot's performance, and hence, identifying the type of loading is important for optimal operation. Different types of loads such as uniform loads that are defined as a force that is applied evenly over the distance of a support. For the least amount of deflection possible, this load is distributed over the entire length of the support [10] such as weight of evenly distributed objects, like boxes, point loads which is a force that is applied to a concentrated point on the support [10], and dynamic loads. Also known as a 'live load,' which is one that changes in direction, position and magnitude, creating varied forces on a structure such as a barrel half filled with a liquid [62]. This can affect the robot's speed, stability, and energy consumption. For example, unevenly distributed loads can lead to the robot tilting, and heavy loads can result in increased power consumption, causing premature battery drain. Moreover, some types of loads need a special treatment while transporting. For example some material should stay at a certain temperature for proper storage and can only handle a certain amount of time out of this temperature.

The type of delivery should also be determined, whether it will be automatic which refers to an AMR delivering goods without the need for human intervention [25] or manual which refers to an AMR delivering goods to or while being operated by a human operator [32]. This information helps in optimizing the delivery process and ensuring that the load is delivered in the required condition.

Thereafter, the robot should be aware about the time range of each operation. While calculating the time and duration is a separate scheduling procedure, the robot must abide by its outputs such as due date, load availability date, and macimuum delivery. The due date, which refers to the scheduled times when each operation or event should be performed or executed in a process [36], and the load availability date, which is the period of time required for the work or activity, or the date of the work or activity to be available for start or finish [61]. These dates are critical in scheduling the robot's operation to ensure that the load is delivered on time.

The maximum delivery time should also be considered for perishable goods or semi-finite products. This information helps in optimizing the delivery process and ensuring that the load is delivered within the required time frame and no damage will occur on the load by the effect of time due to the nature of the load.

Further more, it should also be aware of the nature of the load including the load features. They include information on the load such as its surface, volume, weight, and priority, should be provided to the robot. This
# Navigation Constraints

In this chapter we are going to explore the different constraints for AMRs in intralogistics operations. It covers various factors coming from logistics sector such as precise identification of loading and delivery points, types of loads and deliveries, time constraints, load features, interaction with operators, desired speed. Moreover social and ergonomic constraints are considered such as adhering to social rules, and promoting safe and comfortable human-robot interaction. Due to the limited time of the internship we mentioned all relevant constraints but not all of them are implemented in our approach. This serves the proper understanding and formalization of the problem. In the approach section implemented constraints are pointed out.

## 3.1 Logistics

To ensure efficient and effective operation, an AMR should comply with several constraints projected from the logistic nature of the operation. Firstly, the location of the loading station, which is the point where the robot will pick up the load, should be precisely identified and programmed into the robot's system. This is important to ensure that the robot goes to the right location to pick up the load and minimize the time taken for the operation.

The delivery point, which is the location where the robot will drop off the load, should also be identified and programmed into the robot's system. This information helps the robot to navigate to the correct location, and it also helps in minimizing the time taken for the delivery.

After identifying the loading-unloading position the type of load and delivery should be defined. The type of load carried by an AMR has a significant impact on the robot's performance, and hence, identifying the type of loading is important for optimal operation. Different types of loads such as uniform loads that are defined as a force that is applied evenly over the distance of a support. For the least amount of deflection possible, this load is distributed over the entire length of the support [10] such as weight of evenly distributed objects, like boxes, point loads which is a force that is applied to a concentrated point on the support [10], and dynamic loads. Also known as a 'live load,' which is one that changes in direction, position and magnitude, creating varied forces on a structure such as a barrel half filled with a liquid [62]. This can affect the robot's speed, stability, and energy consumption. For example, unevenly distributed loads can lead to the robot tilting, and heavy loads can result in increased power consumption, causing premature battery drain. Moreover, some types of loads need a special treatment while transporting. For example some material should stay at a certain temperature for proper storage and can only handle a certain amount of time out of this temperature.

The type of delivery should also be determined, whether it will be automatic which refers to an AMR delivering goods without the need for human intervention [25] or manual which refers to an AMR delivering goods to or while being operated by a human operator [32]. This information helps in optimizing the delivery process and ensuring that the load is delivered in the required condition.

Thereafter, the robot should be aware about the time range of each operation. While calculating the time and duration is a separate scheduling procedure, the robot must abide by its outputs such as due date, load availability date, and macimuum delivery. The due date, which refers to the scheduled times when each operation or event should be performed or executed in a process [36], and the load availability date, which is the period of time required for the work or activity, or the date of the work or activity to be available for start or finish [61]. These dates are critical in scheduling the robot's operation to ensure that the load is delivered on time.

The maximum delivery time should also be considered for perishable goods or semi-finite products. This information helps in optimizing the delivery process and ensuring that the load is delivered within the required time frame and no damage will occur on the load by the effect of time due to the nature of the load.

Further more, it should also be aware of the nature of the load including the load features. They include information on the load such as its surface, volume, weight, and priority, should be provided to the robot. This
information is necessary to ensure that the robot selects the appropriate method of transportation and that the load is delivered safely and securely[59].

The robot should also be programmed to interact with operators during loading and unloading operations, facilitating access to the load and ensuring that the operation is done safely and efficiently.

Finally, In addition to the above constraints, the desired speed of the robot should be set. This setting can be useful in the same way as the maximum allowed speed, but with this setting, the robot will drive faster than the set desired speed in a speed zone that requires it, ensuring that the robot operates at maximum efficiency[15].

## 3.2 Social

Navigation is a basic skill for autonomous robots. In the last years human-robot interaction has become an important research field that spans robots' capabilities including perception, reasoning, learning, manipulation and navigation. For navigation, the presence of humans requires novel approaches taking into account the constraints of human comfort and social rules [34]. The aim of defining these constraints is to ensure more socially legible trajectories and reduce the number of times the robot stops due to the proximity of humans. [6].

### 3.2.1 Personal Space

Starting with the individual consideration influenced by the classical proxemics approaches. The concept of a virtual personal space around a person that humans mutually respect - called "Proxemics" - was proposed by Edward T. Hall [20]. These constraints are the distance to humans which is used to penalize reductions in the relative distance between humans and robots and the arc around the human-robot which is used to create a penalty arc around the human, these are penalized spaces for the robot on the sides of the human[6]. Figure 3.1 shows an environment with people that are not interacting explicitly. There is a natural arrangement of people motivated by the respect of individual personal spaces which are represented using circles (as proposed by Hall) although various shapes have been proposed by other authors, as illustrated in Fig. 3.1[53].

<figure><img src="image_11.png" /><figcaption>Figure 3.1: Different shapes of personal space: a Concentric circles. b Egg shape, bigger in the front. c Ellipse shape. d Shape smaller in the dominant side [53]</figcaption></figure>

### 3.2.2 Information Process Space

Another social constraint is the information process space IPS. An IPS is the space within which all objects are considered as potential obstacles when a pedestrian is planning future trajectories [30]. Experiments show that the information process space would have a cone shape instead of semicircular as proposed in similar works. Experiments consisted in collecting the gaze patterns of walking pedestrians. The results point out that the information process space is located in the exact front with a small relative lateral distance. Moreover the subjects in the study do not pay attention in the zone with an angle more than 45 degrees from the walking direction. Figure 3.2 shows a schema showing the information process space characteristics.

### 3.2.3 Interaction Space

similarly to personal spaces formed around a single person, a group of people also result in forming a wider spaces. This space is denoted by interaction space or more specifically the O-space concept. The O-Space is the joint or shared area dedicated to the main activity established by groups in focused interaction. Only participants can enter into it, they protect it and others tend to respect it, [28]. The O-Space geometrical characteristics depend on body size, posture, position and orientation of participants during the activity. Spatial patterns adopted by people in conversation act like social cues to inform pedestrians about the activity. Figure 3.3c illustrates how
information is necessary to ensure that the robot selects the appropriate method of transportation and that the load is delivered safely and securely[59].

The robot should also be programmed to interact with operators during loading and unloading operations, facilitating access to the load and ensuring that the operation is done safely and efficiently.

Finally, In addition to the above constraints, the desired speed of the robot should be set. This setting can be useful in the same way as the maximum allowed speed, but with this setting, the robot will drive faster than the set desired speed in a speed zone that requires it, ensuring that the robot operates at maximum efficiency[15].

## 3.2 Social

Navigation is a basic skill for autonomous robots. In the last years human-robot interaction has become an important research field that spans robots' capabilities including perception, reasoning, learning, manipulation and navigation. For navigation, the presence of humans requires novel approaches taking into account the constraints of human comfort and social rules [34]. The aim of defining these constraints is to ensure more socially legible trajectories and reduce the number of times the robot stops due to the proximity of humans. [6].

### 3.2.1 Personal Space

Starting with the individual consideration influenced by the classical proxemics approaches. The concept of a virtual personal space around a person that humans mutually respect - called “Proxemics” - was proposed by Edward T. Hall [20]. These constraints are the distance to humans which is used to penalize reductions in the relative distance between humans and robots and the arc around the human-robot which is used to create a penalty arc around the human, these are penalized spaces for the robot on the sides of the human [6]. Figure 3.1 shows an environment with people that are not interacting explicitly. There is a natural arrangement of people motivated by the respect of individual personal spaces which are represented using circles (as proposed by Hall) although various shapes have been proposed by other authors, as illustrated in Fig. 3.1 [53].

<figure><img src="image_11.png" /><figcaption>Figure 3.1: Different shapes of personal space: a Concentric circles. b Egg shape, bigger in the front. c Ellipse shape. d Shape smaller in the dominant side [53]</figcaption></figure>

### 3.2.2 Information Process Space

Another social constraint is the information process space IPS. An IPS is the space within which all objects are considered as potential obstacles when a pedestrian is planning future trajectories [30]. Experiments show that the information process space would have a cone shape instead of semicircular as proposed in similar works. Experiments consisted in collecting the gaze patterns of walking pedestrians. The results point out that the information process space is located in the exact front with a small relative lateral distance. Moreover the subjects in the study do not pay attention in the zone with an angle more than 45 degrees from the walking direction. Figure 3.2 shows a schema showing the information process space characteristics.

### 3.2.3 Interaction Space

similarly to personal spaces formed around a single person, a group of people also result in forming a wider spaces. This space is denoted by interaction space or more specifically the O-space concept. The O-Space is the joint or shared area dedicated to the main activity established by groups in focused interaction. Only participants can enter into it, they protect it and others tend to respect it, [28]. The O-Space geometrical characteristics depend on body size, posture, position and orientation of participants during the activity. Spatial patterns adopted by people in conversation act like social cues to inform pedestrians about the activity. Figure 3.3c illustrates how
<figure><img src="image_12.png" /><figcaption>Figure 3.2: The Information Process Space shape according to [30]. A pedestrian is more interested in the exact front to detect obstacles and other pedestrians in order to calculate his next moves. b Shape and measures obtained for the IPS. The pedestrian is represented by the circle [53]</figcaption></figure>

the position and orientation of people can help to decide what groups are in conversation and where would be located the O-Space. Social robots can take benefit from that knowledge to identify social interactions in the close environment [53]. Surrounding the o-space there exists the p-space which is a space used for the placement of the participant and their personal belongings [28].

### 3.2.4 Activity and Affordance Space

in addition human activity possess a virtual space that is recognised and respected by others. This fact adds two more concepts to the social aware navigation strategy. These concepts are activity space and affordance space. The activity space is a social space linked to actions performed by agents. The notion implies a geometric space but does not give an explicit definition for the shape. It can take multiple shapes depending on specific actions. The Activity Space is a social space linked to actions performed by agents. The notion implies a geometric space but does not give an explicit definition for the shape. It can take multiple shapes depending on specific actions [38]. Figure 3.3a illustrates an example of an activity space were can see a woman taking a picture of certain object. Here the space separating between her and the object she is interacting with becomes an activity space. Besides, The Affordance Space is a social space related to a potential activity provided by the environment. In other words, Affordance Spaces are potential Activity Spaces [53]. Figure 3.3b exhibits the picture of a bus schedule. This schedule affords a potential activity for the passing by humans. Therefore, the space in front of this information becomes an affordance space.

<figure><img src="image_13.png" /><figcaption>Figure 3.3: Activity, Interaction and affordance spaces. In a a woman is taking a picture, the space between her and her objective becomes an activity space. In b the bus schedule represents an affordance for humans and the space in front of this information becomes an affordance space. The picture showing a group of people shows spatial patterns of arrangement formed by people interacting in groups. The O-Spaces and p-Spaces are represented respectively by white and red circles [53].</figcaption></figure>

## 3.3 Ergonomic

Regarding the ergonomics of mobile robots, the term "mobile ergonomics" refers to the ergonomic design of mobile devices such as smartphones that usually deals with simplicity, effectiveness and usability, the ergonomics of mobile robots can refer to several aspects related to the design and operation of AMRs. In our application we will be focusing on the operational part of the ergonomics of the design. The behavior of people sharing spaces
<figure><img src="image_12.png" /><figcaption>Figure 3.2: The Information Process Space shape according to [30]. A pedestrian is more interested in the exact front to detect obstacles and other pedestrians in order to calculate his next moves. b Shape and measures obtained for the IPS. The pedestrian is represented by the circle [53]</figcaption></figure>

the position and orientation of people can help to decide what groups are in conversation and where would be located the O-Space. Social robots can take benefit from that knowledge to identify social interactions in the close environment [53]. Surrounding the o-space there exists the p-space which is a space used for the placement of the participant and their personal belongings [28].

### 3.2.4 Activity and Affordance Space

in addition human activity possess a virtual space that is recognised and respected by others. This fact adds two more concepts to the social aware navigation strategy. These concepts are activity space and affordance space. The activity space is a social space linked to actions performed by agents. The notion implies a geometric space but does not give an explicit definition for the shape. It can take multiple shapes depending on specific actions. The Activity Space is a social space linked to actions performed by agents. The notion implies a geometric space but does not give an explicit definition for the shape. It can take multiple shapes depending on specific actions [38]. Figure 3.3a illustrates an example of an activity space were can see a woman taking a picture of certain object. Here the space separating between her and the object she is interacting with becomes an activity space. Besides, The Affordance Space is a social space related to a potential activity provided by the environment. In other words, Affordance Spaces are potential Activity Spaces [53]. Figure 3.3b exhibits the picture of a bus schedule. This schedule affords a potential activity for the passing by humans. Therefore, the space in front of this information becomes an affordance space.

<figure><img src="image_13.png" /><figcaption>Figure 3.3: Activity, Interaction and affordance spaces. In a a woman is taking a picture, the space between her and her objective becomes an activity space. In b the bus schedule represents an affordance for humans and the space in front of this information becomes an affordance space. The picture showing a group of people shows spatial patterns of arrangement formed by people interacting in groups. The O-Spaces and p-Spaces are represented respectively by white and red circles [53].</figcaption></figure>

## 3.3 Ergonomic

Regarding the ergonomics of mobile robots, the term "mobile ergonomics" refers to the ergonomic design of mobile devices such as smartphones that usually deals with simplicity, effectiveness and usability, the ergonomics of mobile robots can refer to several aspects related to the design and operation of AMRs. In our application we will be focusing on the operational part of the ergonomics of the design. The behavior of people sharing spaces
with robots is not so different from with other people. This means that a set of rules also governs the physical behavior of the robot around humans. This can be defined by two major factors the speed and approach method.

### 3.3.1 Comfort Speed

Starting with the speed, A normal walking speed for young human is set to be about 1m/s.alongside with experiments done in [5], this speed when the robot moves in its range it is considered to be fast and uncomfortable by the humans around on the contrary, The comfortable speeds are between 0.254m/s and 0.381m/s. This indicates that humans prefer slower speed for a robot.

### 3.3.2 Approach Method

Now for the approach method. This factor has various approaches, in which one of them is the direct versus indirect approach for the robot to the human. The indirect approach is defined by [5] as a sequence of behaviors were the robot moves toward the subject and began to arc to the left. It would continue this arc for two seconds and then would arc right for two seconds. These actions would cause the robot to appear to be on a course to move past the subject slightly to the right. whereas the direct approach is defined as the movement of the robot heading directly to the subject. knowing so, an indirect approach seemed to be preferred by experimented subjects. An indirect approach is a less threatening behavior because the threat of contact has been reduced as shown in figure 3.4.

<figure><img src="image_14.png" /><figcaption>Figure 3.4: Direct versus Indirect Approaches</figcaption></figure>

### 3.3.3 Acceleration

In addition ergonomic constraints are to be considered during the collaboration process between the robot and the operator to avoid injuries. First, Forceful exertions can be harmful. Tasks should involve smooth force exertions, with the avoidance of sudden or jerky movements.

### 3.3.4 Passing Region

Handling precision (accurate picking and placement), and the type and nature of the grip can introduce additional muscular activation [12]. When the robot delivers the packages the horizontal distance between the body and load should not be greater than full arm reach (>63 cm) as shown in figure 3.5a. Moreover, the position of the robot in either loading or unloading should not result in Extreme body twisting (to either side by more than 45°) without moving the feet as shown in figure 3.5b.

<figure><img src="image_15.png" /><figcaption>(a) Maximum Distance between robot and Body</figcaption></figure>

<figure><img src="image_16.png" /><figcaption>(b) Rotation Range Between Robot and Body</figcaption></figure>

<caption>Figure 3.5: Ergonomics of Manual Material Handling</caption>
with robots is not so different from with other people. This means that a set of rules also governs the physical behavior of the robot around humans. This can be defined by two major factors the speed and approach method.

### 3.3.1 Comfort Speed

Starting with the speed, A normal walking speed for young human is set to be about 1m/s.alongside with experiments done in [5], this speed when the robot moves in its range it is considered to be fast and uncomfortable by the humans around on the contrary, The comfortable speeds are between 0.254m/s and 0.381m/s. This indicates that humans prefer slower speed for a robot.

### 3.3.2 Approach Method

Now for the approach method. This factor has various approaches, in which one of them is the direct versus indirect approach for the robot to the human. The indirect approach is defined by [5] as a sequence of behaviors were the robot moves toward the subject and began to arc to the left. It would continue this arc for two seconds and then would arc right for two seconds. These actions would cause the robot to appear to be on a course to move past the subject slightly to the right. whereas the direct approach is defined as the movement of the robot heading directly to the subject. knowing so, an indirect approach seemed to be preferred by experimented subjects. An indirect approach is a less threatening behavior because the threat of contact has been reduced as shown in figure 3.4.

<figure><img src="image_14.png" /><figcaption>Figure 3.4: Direct versus Indirect Approaches</figcaption></figure>

### 3.3.3 Acceleration

In addition ergonomic constraints are to be considered during the collaboration process between the robot and the operator to avoid injuries. First, Forceful exertions can be harmful. Tasks should involve smooth force exertions, with the avoidance of sudden or jerky movements.

### 3.3.4 Passing Region

Handling precision (accurate picking and placement), and the type and nature of the grip can introduce additional muscular activation [12]. When the robot delivers the packages the horizontal distance between the body and load should not be greater than full arm reach (>63 cm) as shown in figure 3.5a. Moreover, the position of the robot in either loading or unloading should not result in Extreme body twisting (to either side by more than 45°) without moving the feet as shown in figure 3.5b.

<figure><img src="image_15.png" /><figcaption>(a) Maximum Distance between robot and Body</figcaption></figure>

<figure><img src="image_16.png" /><figcaption>(b) Rotation Range Between Robot and Body</figcaption></figure>

<caption>Figure 3.5: Ergonomics of Manual Material Handling</caption>
# State-of-the-Art Of Material Handling Operations

The field of mobile robot navigation in intralogistics has seen significant advancements through various studies and research endeavors. These studies have approached the problem from different angles, considering management aspects, human-robot interaction, and navigation strategies.

In terms of managing mobile robots in intralogistics, researchers have explored the optimization and efficiency of scheduling systems. The work on [14] emphasizes the utilization of Autonomous Mobile Robots (AMRs) to enhance scheduling systems, leading to increased flexibility and productivity in the industry. In our work we are addressing material handling process in intralogistics. This process is divided into transfer and loading and unloading operation.

## 4.1 Transfer Operation

As defined previously Transfer operation is defined as the process of navigating through a defined environment abiding by different constraints. Different studies were conducted in this field where these different constraints were handled diversely.

### 4.1.1 Human Consideration

Transfer operations in intralogistic environment involves direct interaction between the human operator and the robot. This interaction is one of the points that is addressed upon developing any navigation algorithm. To address the close interaction between mobile robots and humans in shared spaces, the study of human-robot interaction has become crucial. Different approaches have been proposed to integrate robots into human environments. One approach treats humans as obstacles without specific constraints, as seen in the research on [46]. Another approach considers humans as dynamic obstacles, predicting their trajectories over a time horizon and planning robot movements accordingly, as demonstrated in [70]. Furthermore, the concept of social navigation incorporates special constraints for humans derived from proxemics theory. This approach, detailed in [53] considers factors such as comfort and integrates them into the navigation constraints of the robot, as exemplified by [51].

### 4.1.2 Transfer Motion Planners

Once the consideration of human presence and constraints in the environment is established, selecting an appropriate navigation strategy becomes crucial. Navigation strategies can be categorized into model-based and machine learning-based approaches. Model-based methods include the dynamic window approach, which calculates feasible robot motions within a given time horizon, as defined in [13]. Other model-based methods, such as the Elastic Band and Time Elastic Band, create virtual elastic bands to guide the robot's path, as discussed in [46] and [11]. Machine learning-based methods involve training the robot to adapt its behavior based on extensive training data, enabling it to navigate in a manner consistent with human behavior.

## 4.2 Handover Operation

On the other hand handover operation is the process of navigating in a certain area for the aim of delivering or receiving certain goods. Papers dealing with handover operations in intralogistics deals with the operation considering different categories such as the type of the robot, type of handover operation.
# State-of-the-Art Of Material Handling Operations

The field of mobile robot navigation in intralogistics has seen significant advancements through various studies and research endeavors. These studies have approached the problem from different angles, considering management aspects, human-robot interaction, and navigation strategies.

In terms of managing mobile robots in intralogistics, researchers have explored the optimization and efficiency of scheduling systems. The work on [14] emphasizes the utilization of Autonomous Mobile Robots (AMRs) to enhance scheduling systems, leading to increased flexibility and productivity in the industry. In our work we are addressing material handling process in intralogistics. This process is divided into transfer and loading and unloading operation.

## 4.1 Transfer Operation

As defined previously Transfer operation is defined as the process of navigating through a defined environment abiding by different constraints. Different studies were conducted in this field where these different constraints were handled diversely.

### 4.1.1 Human Consideration

Transfer operations in intralogistic environment involves direct interaction between the human operator and the robot. This interaction is one of the points that is addressed upon developing any navigation algorithm. To address the close interaction between mobile robots and humans in shared spaces, the study of human-robot interaction has become crucial. Different approaches have been proposed to integrate robots into human environments. One approach treats humans as obstacles without specific constraints, as seen in the research on [46]. Another approach considers humans as dynamic obstacles, predicting their trajectories over a time horizon and planning robot movements accordingly, as demonstrated in [70]. Furthermore, the concept of social navigation incorporates special constraints for humans derived from proxemics theory. This approach, detailed in [53] considers factors such as comfort and integrates them into the navigation constraints of the robot, as exemplified by [51].

### 4.1.2 Transfer Motion Planners

Once the consideration of human presence and constraints in the environment is established, selecting an appropriate navigation strategy becomes crucial. Navigation strategies can be categorized into model-based and machine learning-based approaches. Model-based methods include the dynamic window approach, which calculates feasible robot motions within a given time horizon, as defined in [13]. Other model-based methods, such as the Elastic Band and Time Elastic Band, create virtual elastic bands to guide the robot's path, as discussed in [46] and [11]. Machine learning-based methods involve training the robot to adapt its behavior based on extensive training data, enabling it to navigate in a manner consistent with human behavior.

## 4.2 Handover Operation

On the other hand handover operation is the process of navigating in a certain area for the aim of delivering or receiving certain goods. Papers dealing with handover operations in intralogistics deals with the operation considering different categories such as the type of the robot, type of handover operation.
4.2.1 Handover Type

Handover operations can be categorized into two main types. The first type is when the handler is a machine and no human intervention is required like in [1]. Here kiva robot completes the handover mission without any human intervention were it moves the plates around independent from any human intervention. The other type of handover is the manual handover where the human is required to load and unload the materials to the robot as in [51] where ARCO robot is navigating towards a predefined loading position and waits for the handler to perform the required operation. Another example of manual handover is what is done in [41] where human kinematics are considered in the navigation problem and the AMR is delivering the load directly to the handler taking into consideration the constraints coming from the motion constraints of handover operation and the comfort constraints projected from the presence of human robot interaction.

4.2.2 Type Of the Robot

In handover operations two main robots are considered mobile robots and mobile manipulators. Both Mobile robots and mobile manipulator robots in handover operations are the robots assigned to deliver or receive a certain load with a one main difference is that mobile manipulators are equipped with a robot arm and can perform more complex delivery tasks. For mobile manipulators as in [40] constraints were represented through cost functions depending respectively on the kinematic of the model, field of view, and accessibility. These constraints requires the presence of a full model of the skeleton of the human were these costs are mapped around for mobile robots such as in [51] the robot is also set to abide by certain constraints such as respecting personal spaces and safety constraints defined by speed and distance from the operator. It is assigned to navigate to a certain point in the handover region and the human operator afterwards will approach the robot and proceed with the handover task. In the previous paper the robot keeps a 1.2m distance from the human before reaching the handover point.

4.2.3 Handover Motion Planners

A different set of constraints are to be considered for handover operation. Therefore a different navigation methods are applied. A common way to tackle this problem is done as the work done by [57] [56] the motion planner explicitly takes into account some of these constraints such as relative distance, human's field of view and posture. Here these constraints are represented through cost functions. This representation leads to costmaps defined over the workspace. Motion planning is solved by either considering grid search techniques as in [23] or sampling based techniques as in [7]. Sampling based algorithms enables planning in the robot's configuration space and finding human aware motions in a dynamic environment.

In the work done in [57] two common searched based algorithms are considered and compared their performance based on time. These two algorithms are RRT and T-RRT. A rapidly exploring random tree (RRT) is an algorithm designed to efficiently search nonconvex, high-dimensional spaces by randomly building a space-filling tree. The tree is constructed incrementally from samples drawn randomly from the search space and is inherently biased to grow towards large unsearched areas of the problem [69]. T-RRT is an RRT variant and tree-based motion planner that takes into consideration state costs to compute low-cost paths that follow valleys and saddle points of the configuration-space costmap [26].

In another work done in [66] they used probabilistic roadmap algorithm (PRM). The probabilistic roadmap planner is a motion planning algorithm in robotics, which solves the problem of determining a path between a starting configuration of the robot and a goal configuration while avoiding collisions. It consists of two phases: a construction and a query phase. In the construction phase, a roadmap (graph) is built, approximating the motions that can be made in the environment. First, a random configuration is created. Then, it is connected to some neighbors, typically either the k nearest neighbors or all neighbors less than some predetermined distance [68].

Finally, in a work done in [39] they used fast marching tree algorithm (FMT) where FMT algorithm performs a 'lazy' dynamic programming recursion on a predetermined number of probabilistically drawn samples to grow a tree of paths, which moves steadily outward in cost-to-arrive space [27]. It explores an environment by propagating wavefronts from initial points. It dynamically updates a tree structure by connecting new points based on their cost, and selects the next point to expand using a combination of current cost and heuristic estimates. The algorithm continues until all desired points are reached or termination conditions are met.
### 4.2.1 Handover Type

Handover operations can be categorized into two main types. The first type is when the handler is a machine and no human intervention is required like in [1]. Here kiva robot completes the handover mission without any human intervention were it moves the plates around independent from any human intervention. The other type of handover is the manual handover where the human is required to load and unload the materials to the robot as in [51] where ARCO robot is navigating towards a predefined loading position and waits for the handler to perform the required operation. Another example of manual handover is what is done in [41] where human kinematics are considered in the navigation problem and the AMR is delivering the load directly to the handler taking into consideration the constraints coming from the motion constraints of handover operation and the comfort constraints projected from the presence of human robot interaction.

### 4.2.2 Type Of the Robot

In handover operations two main robots are considered mobile robots and mobile manipulators. Both Mobile robots and mobile manipulator robots in handover operations are the robots assigned to deliver or receive a certain load with a one main difference is that mobile manipulators are equipped with a robot arm and can perform more complex delivery tasks. For mobile manipulators as in [40] constraints were represented through cost functions depending respectively on the kinematic of the model, field of view, and accessibility. These constraints requires the presence of a full model of the skeleton of the human were these costs are mapped around for mobile robots such as in [51] the robot is also set to abide by certain constraints such as respecting personal spaces and safety constraints defined by speed and distance from the operator. It is assigned to navigate to a certain point in the handover region and the human operator afterwards will approach the robot and proceed with the handover task. In the previous paper the robot keeps a 1.2m distance from the human before reaching the handover point.

### 4.2.3 Handover Motion Planners

A different set of constraints are to be considered for handover operation. Therefore a different navigation methods are applied. A common way to tackle this problem is done as the work done by [57] [56] the motion planner explicitly takes into account some of these constraints such as relative distance, human's field of view and posture. Here these constraints are represented through cost functions. This representation leads to costmaps defined over the workspace. Motion planning is solved by either considering grid search techniques as in [23] or sampling based techniques as in [7]. Sampling based algorithms enables planning in the robot's configuration space and finding human aware motions in a dynamic environment.

In the work done in [57] two common searched based algorithms are considered and compared their performance based on time. These two algorithms are RRT and T-RRT. A rapidly exploring random tree (RRT) is an algorithm designed to efficiently search nonconvex, high-dimensional spaces by randomly building a space-filling tree. The tree is constructed incrementally from samples drawn randomly from the search space and is inherently biased to grow towards large unsearched areas of the problem [69]. T-RRT is an RRT variant and tree-based motion planner that takes into consideration state costs to compute low-cost paths that follow valleys and saddle points of the configuration-space costmap [26].

In another work done in [66] they used probabilistic roadmap algorithm (PRM). The probabilistic roadmap planner is a motion planning algorithm in robotics, which solves the problem of determining a path between a starting configuration of the robot and a goal configuration while avoiding collisions. It consists of two phases: a construction and a query phase. In the construction phase, a roadmap (graph) is built, approximating the motions that can be made in the environment. First, a random configuration is created. Then, it is connected to some neighbors, typically either the k nearest neighbors or all neighbors less than some predetermined distance [68].

Finally, in a work done in [39] they used fast marching tree algorithm (FMT) where FMT algorithm performs a 'lazy' dynamic programming recursion on a predetermined number of probabilistically drawn samples to grow a tree of paths, which moves steadily outward in cost-to-arrive space [27]. It explores an environment by propagating wavefronts from initial points. It dynamically updates a tree structure by connecting new points based on their cost, and selects the next point to expand using a combination of current cost and heuristic estimates. The algorithm continues until all desired points are reached or termination conditions are met.
Handover Problem Specification

As mentioned in the problem specification part the problem consists of transfer operation and loading-unloading operation. For both operations a model of the robot should be considered. In this section a model of the robot and the environment will be formalized.

The robot moves in a workspace $W \in \mathbb{R}^2$ which describes all the area surrounding the robot. The workspace $W$ consists of two areas the blocked areas and the free areas. The blocked area includes the objects' occupied areas denoted $O = \{O_1, O_2, \dots, O_N\}$ where $N$ is the $N^{th}$ object's occupied area, the people denoted by $P = \{P_1, P_2, \dots, P_M\}$ were $M$ is the $M^{th}$ person's occupied area, where $P_i$ is the $i^{th}$ person and $O_j$ is the $j^{th}$ object's occupied area or the Handler $H$ whose defined as a person that is performing either loading or unloading operation. A pedestrian, denoted by $P_t$, is defined by his configuration variables $s_{P_t} = [x_{P_t} \ y_{P_t} \ \theta_{P_t}]^T$, which respectively denote the position in the x-direction, position in the y-direction, and body direction. The handler, denoted by $H$, is defined by their configuration variables $s_H = [x_H \ y_H \ \theta_H; \psi; \phi]^T$, representing the position in the x-direction, position in the y-direction, body direction, gaze direction around z-axis and gaze direction around y-axis. The volume around the handler is modeled as a cylinder that circumscribes the handler and is defined by its radius $\rho_H$ and height $\eta_H$, corresponding to the height of the human handler as shown in figure. The occupied areas resemble the areas occupied by an existing obstacles and defined by their union $P \cup O \cup H$. The remaining space is free space denoted $F$. This space represents the summation of all the spaces that the robot can navigate in. It is defined as the complementary of the blocked space and can be represented as $F = W - \{P \cup O \cup H\}$. Some of the free spaces the robot can navigate in posses some structural elements that defines the behavior of the robot in the environment. Loading and unloading areas are one of these areas in which the robot should collaborate with a human handler $H$ denoted as $L \subset W$.

In our case a differential drive robot is used which is a non-holonomic system. This is a drive system that uses two independently controlled wheels, one on each side of the robot. By varying the relative speed and direction of rotation of the two wheels, the robot can turn, move forward or backward, and follow curved paths. Figure 5.2 shows the representation of a differential robot whose state of the robot $s_R = [x \ y \ \theta]^T$ represents the position of the robot in x coordinates, the position of the robot in y coordinates, and the orientation of the robot, and the control input $\mu = [v \ \omega]$ represents the linear velocity of the robot and the angular velocity around its center respectively. The general motion model of the robot is described as a function of the state of the robot and the control input $\dot{s} = f(s, \mu)$.

The interaction between the robot and the surrounding environment imposes multiple constraints on the robot's behavior such as performing obstacle avoidance and respecting comfort speed, personal spaces, field of view and passing region.

<figure><img src="image_17.png" /><figcaption>Figure 5.1: Modeling the Handler's Surrounding Volume as a Circumscribing Cylinder: Radius and Height</figcaption></figure>
# Handover Problem Specification

As mentioned in the problem specification part the problem consists of transfer operation and loading-unloading operation. For both operations a model of the robot should be considered. In this section a model of the robot and the environment will be formalized.

The robot moves in a workspace $W \in \mathbb{R}^2$ which describes all the area surrounding the robot. The workspace $W$ consists of two areas the blocked areas and the free areas. The blocked area includes the objects' occupied areas denoted $O = \{O_1, O_2, \dots, O_N\}$ where $N$ is the $N^{th}$ object's occupied area, the people denoted by $P = \{P_1, P_2, \dots, P_M\}$ were $M$ is the $M^{th}$ person's occupied area, where $P_i$ is the $i^{th}$ person and $O_j$ is the $j^{th}$ object's occupied area or the Handler $H$ whose defined as a person that is performing either loading or unloading operation. A pedestrian, denoted by $P_t$, is defined by his configuration variables $s_{P_t} = [x_{P_t} \ y_{P_t} \ \theta_{P_t}]^T$, which respectively denote the position in the x-direction, position in the y-direction, and body direction. The handler, denoted by $H$, is defined by their configuration variables $s_H = [x_H \ y_H \ \theta_H; \psi; \phi]^T$, representing the position in the x-direction, position in the y-direction, body direction, gaze direction around z-axis and gaze direction around y-axis. The volume around the handler is modeled as a cylinder that circumscribes the handler and is defined by its radius $\rho_H$ and height $\eta_H$, corresponding to the height of the human handler as shown in figure. The occupied areas resemble the areas occupied by an existing obstacles and defined by their union $P \cup O \cup H$. The remaining space is free space denoted $F$. This space represents the summation of all the spaces that the robot can navigate in. It is defined as the complementary of the blocked space and can be represented as $F = W - \{P \cup O \cup H\}$. Some of the free spaces the robot can navigate in posses some structural elements that defines the behavior of the robot in the environment. Loading and unloading areas are one of these areas in which the robot should collaborate with a human handler $H$ denoted as $L \subset W$.

In our case a differential drive robot is used which is a non-holonomic system. This is a drive system that uses two independently controlled wheels, one on each side of the robot. By varying the relative speed and direction of rotation of the two wheels, the robot can turn, move forward or backward, and follow curved paths. Figure 5.2 shows the representation of a differential robot whose state of the robot $s_R = [x \ y \ \theta]^T$ represents the position of the robot in x coordinates, the position of the robot in y coordinates, and the orientation of the robot, and the control input $\mu = [v \ \omega]$ represents the linear velocity of the robot and the angular velocity around its center respectively. The general motion model of the robot is described as a function of the state of the robot and the control input $\dot{s} = f(s, \mu)$.

The interaction between the robot and the surrounding environment imposes multiple constraints on the robot's behavior such as performing obstacle avoidance and respecting comfort speed, personal spaces, field of view and passing region.

<figure><img src="image_17.png" /><figcaption>Figure 5.1: Modeling the Handler's Surrounding Volume as a Circumscribing Cylinder: Radius and Height</figcaption></figure>
<figure><img src="image_18.png" /><figcaption>Figure 5.2: Differential Robot Representation</figcaption></figure>

## 5.1 Collision Avoidance

Collision avoidance is the problem of manoeuvring the robots from their initial positions to predefined targets while avoiding collisions[45]. To achieve this goal the robot is allowed to navigate in free spaces $F$ and avoid any intersection between the robot at any time $t$ with obstacles, people, or handlers $R(t) \cap \{P(t) \cup H(t) \cup O(t)\} = \emptyset$.

## 5.2 Speed

In a robot navigation problem where human comfort is not taken into account, the robot's speed $v$ is bounded by the upper and lower bound of actual physical limit of the robot's speed $v_{min} \le v \le v_{max}$. In intra-logistics the coexistence of humans and the robot in the same environment is inevitable. Therefore, the robot's speed is bounded by the speed that humans consider comfortable. Experiments indicate that human subjects feel uncomfortable only with the robots' fast approach speed. The comfortable speeds $v_{comfort}$ are between 0.254m/s and 0.381m/s while the uncomfortable fast speed is higher than $v_{comfort}$. Normal walking speed for young human is about 1m/s, suggesting that humans prefer slower speed for a robot $v \le v_{comfort}$ where $v_{comfort} = 0.381$ is maximum value of the comfort range defined [53].

## 5.3 Personal Space

The interaction between humans and robots can be divided into two basic situations, namely (i) the case where the robot must perform a task whilst reducing the possible social impact, and (ii) the case where the task involves interacting directly with a person. Either way, there are some nonverbal social rules implied that must be respected, especially those related to personal space of individuals, rules which are studied in proxemics [65]. The personal space is modelled by a asymmetric 2-dimensional Gaussian Function[29], which associates the distance between the point $k = [x_k y_k]^T \in W$ where $x_k$ and $y_k$ is the x position and the y position of point $k$ and the person's position with a real value $g_i \in [0, 1]$ denoted as $[x_p y_p]^T \in W$ where $x_p$ and $y_p$ is the x position and the y position of point $p$. The expression for the Gaussian function is

$$g_i(x_k, y_k) = \exp\left(-\left(a(\theta_p)(x_k - x_p)^2 + b(\theta_p)(x_k - x_p)(y_k - y_p) + c(\theta_p)(y_k - y_p)^2\right)\right) \quad (5.1)$$

where the coefficients a, b and c are used to take into account the orientation $\theta_p$ where $\theta_p$ is the orientation of the person at the present point $[x_p y_p]^T \in W$, and are defined by the relations

$$\begin{align}
a(\theta_p) &= \frac{(\cos(\theta))^2}{2 * \sigma_s^2} + \frac{(\sin(\theta))^2}{2 * \sigma_s^2} \nonumber \\
b(\theta_p) &= \frac{(\sin(2\theta))^2}{4 * \sigma_s^2} - \frac{(\sin(2\theta))^2}{4 * \sigma_s^2} \tag{5.2} \\
c(\theta_p) &= \frac{(\sin(\theta))^2}{2 * \sigma_s^2} + \frac{(\cos(2\theta))^2}{2 * \sigma_s^2} \nonumber
\end{align}$$

where $\sigma_s$ is the variance to the sides ($\theta_p \pm \pi/2$ direction) and $\sigma$ represents the variance along the front and rear direction [29]. Figure 5.3 illustrates the personal space model where the x-axis represent the x position, y-axis represent the y position and the black arrow represents the direction of person given as $\theta_p$. The colors around the present point $[x_p, y_p]^T$ represent the variation of the personal space levels defined in the navigation constraint part. The dark blue circumference represent the end of the social zone and the beginning of the personal space. Beyond this point the robot is penalized if navigating in but still can navigate in. This statement is true while going close to the person until we reach the red circumference which is the beginning of the intimate zone. Beyond this point the robot is forbidden to navigate in.
<figure><img src="image_18.png" /><figcaption>Figure 5.2: Differential Robot Representation</figcaption></figure>

## 5.1 Collision Avoidance

Collision avoidance is the problem of manoeuvring the robots from their initial positions to predefined targets while avoiding collisions[45]. To achieve this goal the robot is allowed to navigate in free spaces $F$ and avoid any intersection between the robot at any time $t$ with obstacles, people, or handlers $R(t) \cap \{P(t) \cup H(t) \cup O(t)\} = \emptyset$.

## 5.2 Speed

In a robot navigation problem where human comfort is not taken into account, the robot's speed $v$ is bounded by the upper and lower bound of actual physical limit of the robot's speed $v_{min} \le v \le v_{max}$. In intra-logistics the coexistence of humans and the robot in the same environment is inevitable. Therefore, the robot's speed is bounded by the speed that humans consider comfortable. Experiments indicate that human subjects feel uncomfortable only with the robots' fast approach speed. The comfortable speeds $v_{comfort}$ are between 0.254m/s and 0.381m/s while the uncomfortable fast speed is higher than $v_{comfort}$. Normal walking speed for young human is about 1m/s, suggesting that humans prefer slower speed for a robot $v \le v_{comfort}$ where $v_{comfort} = 0.381$ is maximum value of the comfort range defined [53].

## 5.3 Personal Space

The interaction between humans and robots can be divided into two basic situations, namely (i) the case where the robot must perform a task whilst reducing the possible social impact, and (ii) the case where the task involves interacting directly with a person. Either way, there are some nonverbal social rules implied that must be respected, especially those related to personal space of individuals, rules which are studied in proxemics [65]. The personal space is modelled by a asymmetric 2-dimensional Gaussian Function[29], which associates the distance between the point $k = [x_k y_k]^T \in W$ where $x_k$ and $y_k$ is the x position and the y position of point k and the person's position with a real value $g_i \in [0, 1]$ denoted as $[x_p y_p]^T \in W$ where $x_p$ and $y_p$ is the x position and the y position of point p. The expression for the Gaussian function is

$$g_i(x_k, y_k) = \exp\left(-a(\theta_p)(x_k - x_p)^2 + b(\theta_p)(x_k - x_p)(y_k - y_p) + c(\theta_p)(y_k - y_p)^2\right) \quad (5.1)$$

where the coefficients a, b and c are used to take into account the orientation $\theta_p$ where $\theta_p$ is the orientation of the person at the present point $[x_p y_p]^T \in W$, and are defined by the relations

$$\begin{align}
a(\theta_p) &= \frac{(\cos(\theta))^2}{2 * \sigma_s^2} + \frac{(\sin(\theta))^2}{2 * \sigma_s^2} \nonumber \\
b(\theta_p) &= \frac{(\sin(2\theta))^2}{4 * \sigma_s^2} - \frac{(\sin(2\theta))^2}{4 * \sigma_s^2} \tag{5.2} \\
c(\theta_p) &= \frac{(\sin(\theta))^2}{2 * \sigma_s^2} + \frac{(\cos(2\theta))^2}{2 * \sigma_s^2} \nonumber
\end{align}$$

where $\sigma_s$ is the variance to the sides ($\theta_p \pm \pi/2$ direction) and $\sigma$ represents the variance along the front and rear direction [29]. Figure 5.3 illustrates the personal space model where the x-axis represent the x position, y-axis represent the y position and the black arrow represents the direction of person given as $\theta_p$. The colors around the present point $[x_p, y_p]^T$ represent the variation of the personal space levels defined in the navigation constraint part. The dark blue circumference represent the end of the social zone and the beginning of the personal space. Beyond this point the robot is penalized if navigating in but still can navigate in. This statement is true while going close to the person until we reach the red circumference which is the beginning of the intimate zone. Beyond this point the robot is forbidden to navigate in.
<figure><img src="image_19.png" /><figcaption>Figure 5.3: contour map personal space of a single individual [64] as modelled by Equation 5.2. The person is posed at $p = [0 \ 0 \ 3\pi/4]^T$.</figcaption></figure>

## 5.4 Field of View

The definitions of the field of view of humans contains defining the range of vision of the eye physically and the attention span inside the physical range of the eye. To do so standard references should be taken into consideration. For human anatomy a standard worldwide definition is well established and defined as shown in figure 5.4a and detailed in [16]. Hence we used this common terminology to define the field of view.

<figure><img src="image_20.png" /><figcaption>Figure 5.4: Human Anatomy and Field of View Reference</figcaption></figure>

The physical range of view of the human is defined from angles $\psi$ and $\phi$ representing the rotation of the neck around the vertical axis and frontal horizontal axis respectively and from the type of vision human being have. Type of vision human being has is the binocular vision. In the horizontal plane, human can have a visual range of 120°. According to SAE J985 eye can be turned only to 30 degrees before head turns. In the sagittal plane, eye can be turned up to 15 degrees above and below. It has a capacity to see 45 degrees upwards and 65 degrees downwards whenever necessary [19] as shown in figure 5.4b.

To simplify the problem, we defined the field of view considering the rotation of the neck around the vertical axis $\psi$ and the range of view on the horizontal plane. Therefore the field of view is defined as the semi-circle centered at the center of vertical axis of the human and bounded by the visual limit of left and right eye considering 60° deviation from the standard line of sight $\psi$ in each direction as shown in figure 5.4b.

## 5.5 Passing Region

The robot should perform a handover in a designated region, which is derived from ergonomic considerations mentioned in navigation constraints chapter. First we should start by defining geometric references of the human model then defining the passing region.

### 5.5.1 Human body References

The bio-mechanical model for the assessment of ergonomic constraints for manual material handling is defined by the National Institute for Occupational Safety and Health (NIOSH). The model defines two parameters to take into consideration as shown in figure 5.5. Figure 5.5a illustrates distance of the hands away from the mid-point between the ankles, in inches or centimeters (measure at the origin and destination of lift) denoted by H. This
<figure><img src="image_19.png" /><figcaption>Figure 5.3: contour map personal space of a single individual [64] as modelled by Equation 5.2. The person is posed at $p = [0 \ 0 \ 3\pi/4]^T$.</figcaption></figure>

## 5.4 Field of View

The definitions of the field of view of humans contains defining the range of vision of the eye physically and the attention span inside the physical range of the eye. To do so standard references should be taken into consideration. For human anatomy a standard worldwide definition is well established and defined as shown in figure 5.4a and detailed in [16]. Hence we used this common terminology to define the field of view.

<figure><img src="image_20.png" /><figcaption>Figure 5.4: Human Anatomy and Field of View Reference</figcaption></figure>

The physical range of view of the human is defined from angles $\psi$ and $\phi$ representing the rotation of the neck around the vertical axis and frontal horizontal axis respectively and from the type of vision human being have. Type of vision human being has is the binocular vision. In the horizontal plane, human can have a visual range of 120°. According to SAE J985 eye can be turned only to 30 degrees before head turns. In the sagittal plane, eye can be turned up to 15 degrees above and below. It has a capacity to see 45 degrees upwards and 65 degrees downwards whenever necessary [19] as shown in figure 5.4b.

To simplify the problem, we defined the field of view considering the rotation of the neck around the vertical axis $\psi$ and the range of view on the horizontal plane. Therefore the field of view is defined as the semi-circle centered at the center of vertical axis of the human and bounded by the visual limit of left and right eye considering 60° deviation from the standard line of sight $\psi$ in each direction as shown in figure 5.4b.

## 5.5 Passing Region

The robot should perform a handover in a designated region, which is derived from ergonomic considerations mentioned in navigation constraints chapter. First we should start by defining geometric references of the human model then defining the passing region.

### 5.5.1 Human body References

The bio-mechanical model for the assessment of ergonomic constraints for manual material handling is defined by the National Institute for Occupational Safety and Health (NIOSH). The model defines two parameters to take into consideration as shown in figure 5.5. Figure 5.5a illustrates distance of the hands away from the mid-point between the ankles, in inches or centimeters (measure at the origin and destination of lift) denoted by H. This
distance according to [12] should not exceed 63 cm. Figure 5.5b represents Asymmetry Angle ($\phi$) that represents the angular measure of how far the object is displaced from the front of the worker's body at the beginning or ending of the lift, in degrees (measure at the origin and destination of lift). The asymmetry angle is defined by the location of the load relative to the worker's mid-sagittal plane(The mid-sagittal plane or median plane divides the body into two parts at natural body position (Described the position of the body when the hands are directly in front of the body and there is minimal twisting at the legs,torso, or shoulders.). It vertically splits any object or organism into two relatively equal halves - left and right. In humans, each of the two bisected divisions includes half of the head, thorax, abdomen, one arm and one leg[31].),rather than the position of the feet or the extent of body twist. This angle should not exceed 45°in either directions.

<figure><img src="image_21.png" /></figure>

<figure><img src="image_22.png" /></figure>

<figure><img src="image_23.png" /><figcaption>Figure 5.5: National Institute for Occupational Safety and Health bio-mechanical Model [67]</figcaption></figure>

Based on the previously mentioned constraints a human is modeled using the parameters shown in figure 5.5c.These parameters resembles the coordinates in 2D spaces denoted as x and y, the angle from the forward mid-sagittal plane denoted by A, and the distance of the hands away from the mid-point between the ankles denoted as H.

## 5.5.2 Passing Region

This region is defined as a sector centered at the center of the circular area resembling the handler and bounded by the asymmetry angle $\phi$ within a range of 45 degrees in each direction ($-45 \le \phi \le +45$). The radius of the circular sector, denoted as $r_H$, represents the maximum hand reach of the human at $\phi = 0$. As the absolute value of $\phi$ increases, this distance gradually decreases, signifying a reduced reach for the human handler $r_H(\phi) = r_{Hmax} - (r_H \times \tan(|\phi|))$ as shown in figure 5.6.The model of the human represented in figure 5.6 such that the human is standing in anatomical position.Anatomical position, or standard anatomical position, refers to the positioning of the body when it is standing upright and facing forward with each arm hanging on either side of the body, and the palms facing forward .An extensive description of the position is described in [43].Moreover the figure 5.6 shows the personal space of the handler which is defined as the normal personal space minus the handling region.

<figure><img src="image_24.png" /><figcaption>Figure 5.6: Passing Region of Human Handler</figcaption></figure>
distance according to [12] should not exceed 63 cm. Figure 5.5b represents Asymmetry Angle ($\phi$) that represents the angular measure of how far the object is displaced from the front of the worker's body at the beginning or ending of the lift, in degrees (measure at the origin and destination of lift). The asymmetry angle is defined by the location of the load relative to the worker's mid-sagittal plane(The mid-sagittal plane or median plane divides the body into two parts at natural body position (Described the position of the body when the hands are directly in front of the body and there is minimal twisting at the legs,torso, or shoulders.). It vertically splits any object or organism into two relatively equal halves - left and right. In humans, each of the two bisected divisions includes half of the head, thorax, abdomen, one arm and one leg[31].),rather than the position of the feet or the extent of body twist. This angle should not exceed 45°in either directions.

<figure><img src="image_21.png" /></figure>

<figure><img src="image_22.png" /></figure>

<figure><img src="image_23.png" /><figcaption>Figure 5.5: National Institute for Occupational Safety and Health bio-mechanical Model [67]</figcaption></figure>

Based on the previously mentioned constraints a human is modeled using the parameters shown in figure 5.5c.These parameters resembles the coordinates in 2D spaces denoted as x and y, the angle from the forward mid-sagittal plane denoted by A, and the distance of the hands away from the mid-point between the ankles denoted as H.

## 5.5.2 Passing Region

This region is defined as a sector centered at the center of the circular area resembling the handler and bounded by the asymmetry angle $\phi$ within a range of 45 degrees in each direction ($-45 \le \phi \le +45$). The radius of the circular sector, denoted as $r_H$, represents the maximum hand reach of the human at $\phi = 0$. As the absolute value of $\phi$ increases, this distance gradually decreases, signifying a reduced reach for the human handler $r_H(\phi) = r_{Hmax} - (r_H \times \tan(|\phi|))$ as shown in figure 5.6.The model of the human represented in figure 5.6 such that the human is standing in anatomical position.Anatomical position, or standard anatomical position, refers to the positioning of the body when it is standing upright and facing forward with each arm hanging on either side of the body, and the palms facing forward .An extensive description of the position is described in [43].Moreover the figure 5.6 shows the personal space of the handler which is defined as the normal personal space minus the handling region.

<figure><img src="image_24.png" /><figcaption>Figure 5.6: Passing Region of Human Handler</figcaption></figure>
# Material Handling Navigation Approach

Mobile robot navigation is a challenging problem because environments are often complex, dynamics can be difficult to predict accurately, and both planning time and computational resources are limited. From the above listed constraints we are considering collision avoidance, speed, personal space and passing region in the navigation approach. We can generally model the dynamics of the robot by a nonlinear differential equation of the form $\dot{x} = f(x, u)$ where the input or control vector $u$ and the state vector $x$ are both time-varying points in input and state space, respectively. The state space of a dynamical system is the set of all possible states of the system. Each coordinate is a state variable, and the values of all the state variables completely describes the state of the robot. In other words, each point in the state space corresponds to a different state of the robot. The complexity of such accurate models of mobility leads to a difficult trade-off between the computational demands of perceptive intelligence at the local level and deliberative intelligence at the global level. It is difficult to be both smart and fast when computation is limited.

<figure><img src="image_25.png" /><figcaption>Figure 6.1: Hierarchical Motion Planning [22].</figcaption></figure>

A first step to approach the problem is by considering a loading operation for a static handler model. one approach is employ a hierarchical motion planning architecture to generate behaviors that are both intelligent and responsive. Figure 6.1 illustrates a case in which the environment is a continuous cost field. The architecture is hierarchical in the sense that two levels of detail are used for the modeling of both the robot and the environment. The higher level motion planner (global planner) is responsible for directing the robot to reach the passing region. It produces a large-scale, long-term motion plan based on simplified robot models and coarse representations of the environment. Conversely, the lower level motion planner (local planner) is used to keep the robot safe. It generates a finer scale, short term motion plan based on higher-fidelity robot models and finer-resolution representations of the environment [22]. High fidelity models refer to representations of the environment or the system being controlled that capture a high level of detail and accuracy. These models provide a more realistic depiction of the environment or robot's dynamics, allowing for more precise and accurate planning and control.

The models used by each of these planners are also typically associated with mutually exclusive spatial regions. Local planner models are used in a small region centered around the robot that extends outward as far as useful perception data are available. Global planner models are used outside the boundary of the local region. The local region moves with the robot so that decisions are based on models that are, at least locally, as accurate as computation and sensing will allow. In this approach a search based state lattice planner is used as a global planner and a model predictive control (MPC) is used as a local planner.

## 6.1 Search Based State Lattice Planner

A state lattice planner is a motion planning algorithm commonly used in robotics to generate feasible paths for a mobile agent, such as a robots, in a given environment [48]. Lattice planners are widely used in autonomous
# Material Handling Navigation Approach

Mobile robot navigation is a challenging problem because environments are often complex, dynamics can be difficult to predict accurately, and both planning time and computational resources are limited. From the above listed constraints we are considering collision avoidance, speed, personal space and passing region in the navigation approach. We can generally model the dynamics of the robot by a nonlinear differential equation of the form $\dot{x} = f(x, u)$ where the input or control vector $u$ and the state vector $x$ are both time-varying points in input and state space, respectively. The state space of a dynamical system is the set of all possible states of the system. Each coordinate is a state variable, and the values of all the state variables completely describes the state of the robot. In other words, each point in the state space corresponds to a different state of the robot. The complexity of such accurate models of mobility leads to a difficult trade-off between the computational demands of perceptive intelligence at the local level and deliberative intelligence at the global level. It is difficult to be both smart and fast when computation is limited.

<figure><img src="image_25.png" /><figcaption>Figure 6.1: Hierarchical Motion Planning [22].</figcaption></figure>

A first step to approach the problem is by considering a loading operation for a static handler model, one approach is to employ a hierarchical motion planning architecture to generate behaviors that are both intelligent and responsive. Figure 6.1 illustrates a case in which the environment is a continuous cost field. The architecture is hierarchical in the sense that two levels of detail are used for the modeling of both the robot and the environment. The higher level motion planner (global planner) is responsible for directing the robot to reach the passing region. It produces a large-scale, long-term motion plan based on simplified robot models and coarse representations of the environment. Conversely, the lower level motion planner (local planner) is used to keep the robot safe. It generates a finer scale, short term motion plan based on higher-fidelity robot models and finer-resolution representations of the environment [22]. High fidelity models refer to representations of the environment or the system being controlled that capture a high level of detail and accuracy. These models provide a more realistic depiction of the environment or robot's dynamics, allowing for more precise and accurate planning and control.

The models used by each of these planners are also typically associated with mutually exclusive spatial regions. Local planner models are used in a small region centered around the robot that extends outward as far as useful perception data are available. Global planner models are used outside the boundary of the local region. The local region moves with the robot so that decisions are based on models that are, at least locally, as accurate as computation and sensing will allow. In this approach a search based state lattice planner is used as a global planner and a model predictive control (MPC) is used as a local planner.

## 6.1 Search Based State Lattice Planner

A state lattice planner is a motion planning algorithm commonly used in robotics to generate feasible paths for a mobile agent, such as a robot, in a given environment [48]. Lattice planners are widely used in autonomous
driving. Unlike commonly used probabilistic motion planners such as PRM*, RRT* or FMT, lattices consist of a regular discretization of a robot's state space while ensuring kinematic and dynamic constraints on motions. Lattice planners pre-compute feasible trajectories between a discrete set of robot states. A pair of states together with a trajectory between the states is called a motion primitive a collection of motion primitives forms a control set. Thus, the robot's dynamics do not need to be accounted for during the actual path planning. In Figure 6.2 we illustrate an example of a state lattice using clothoid trajectories, as well as a set of different trajectories between two fixed states obtained by varying the maximum curvature and maximum derivative of curvature[4].

<figure><img src="image_26.png" /><figcaption>Figure 6.2: Examples of a Lattice Control Set and Different Trajectories</figcaption></figure>

### 6.1.1 Search Based Planning

Search-based planning is a motion planning method which uses graph search methods to compute paths or trajectories over a discrete representation of the problem. Because a graph is inherently discrete, all graph-search algorithms require this discrete representation. Search-based planning can then be seen as two problems. First is how to turn the problem into a graph, and second is how to search the graph to find the best solution.

This is in contrast to sample-based planning methods, which are able to solve problems in continuous space without a graph representation (though certain sampling-based methods do discretize the space). The most apparent difference between the two methods are speed and optimality: graph-search methods can guarantee how "efficient" a solution is, but, in general, does so at the expense of computation time. Sampling based planners run fast, but can result in unusual looking and possibly inefficient paths. First we introduce the process of discretizing the environment, then we describe how planning is done.

### 6.1.2 Discretization of State Space

In order to properly represent the problem with a graph, we first discretize all possible states so that there are a finite number of states a robot could be in. Here, the definition of state is presented as $S_R = [x \ y \ \theta]^T$ which defines the x-position, y-position and orientation of the robot. The collection of all possible states is defined as the state space. The figure 6.3 shows an example of a state space. Here, each possible (x,y) position that a robot can be in is represented as a node with a state $S_R \in [S_1, S_2, ..., S_I]$ where $S_I$ represents the $I$-th state the robot can occur in. A path in this state space would then be moving through a series of adjacent states (for example S1, S4, S8).

<figure><img src="image_27.png" /><figcaption>Figure 6.3: Discretization of State Space</figcaption></figure>

The graph representation then comes naturally with valid transitions between adjacent states are represented by edges between the nodes. For now, we assume that any two adjacent states will have a valid transition between them. This oversimplifying of the problem will be detailed in the coming sections.
driving. Unlike commonly used probabilistic motion planners such as PRM*, RRT* or FMT, lattices consist of a regular discretization of a robot's state space while ensuring kinematic and dynamic constraints on motions. Lattice planners pre-compute feasible trajectories between a discrete set of robot states. A pair of states together with a trajectory between the states is called a motion primitive a collection of motion primitives forms a control set. Thus, the robot's dynamics do not need to be accounted for during the actual path planning. In Figure 6.2 we illustrate an example of a state lattice using clothoid trajectories, as well as a set of different trajectories between two fixed states obtained by varying the maximum curvature and maximum derivative of curvature[4].

<figure><img src="image_26.png" /><figcaption>Figure 6.2: Examples of a Lattice Control Set and Different Trajectories</figcaption></figure>

### 6.1.1 Search Based Planning

Search-based planning is a motion planning method which uses graph search methods to compute paths or trajectories over a discrete representation of the problem. Because a graph is inherently discrete, all graph-search algorithms require this discrete representation. Search-based planning can then be seen as two problems. First is how to turn the problem into a graph, and second is how to search the graph to find the best solution.

This is in contrast to sample-based planning methods, which are able to solve problems in continuous space without a graph representation (though certain sampling-based methods do discretize the space). The most apparent difference between the two methods are speed and optimality: graph-search methods can guarantee how "efficient" a solution is, but, in general, does so at the expense of computation time. Sampling based planners run fast, but can result in unusual looking and possibly inefficient paths. First we introduce the process of discretizing the environment, then we describe how planning is done.

### 6.1.2 Discretization of State Space

In order to properly represent the problem with a graph, we first discretize all possible states so that there are a finite number of states a robot could be in. Here, the definition of state is presented as $S_R = [x \ y \ \theta]^T$ which defines the x-position, y-position and orientation of the robot. The collection of all possible states is defined as the state space. The figure 6.3 shows an example of a state space. Here, each possible (x,y) position that a robot can be in is represented as a node with a state $S_R \in [S_1, S_2, ..., S_I]$ where $S_I$ represents the $I$-th state the robot can occur in. A path in this state space would then be moving through a series of adjacent states (for example S1, S4, S8).

<figure><img src="image_27.png" /><figcaption>Figure 6.3: Discretization of State Space</figcaption></figure>

The graph representation then comes naturally with valid transitions between adjacent states are represented by edges between the nodes. For now, we assume that any two adjacent states will have a valid transition between them. This oversimplifying of the problem will be detailed in the coming sections.
### 6.1.3 State Validity and Collision Checking

One important notion about the state space is the validity of states. When computing a trajectory, we need all states in the trajectory to be valid and avoid any collision.

Figure 6.4 represents the 2D environment where we see that there are states that are invalid. These states are invalid if they coexist with a region defined previously as occupied *O* in the environment. The robot is not allowed to be traveling through them. In this case, those states are obviously invalid. More specifically, the robot would be in collision with the occupied regions. Given that, the robot's logic should take into consideration the prior knowledge about the robot's surrounding environment and a given state and returns a feedback telling whether a collision state occurs or not (or synonymously, if the state is valid).

<figure><img src="image_28.png" /><figcaption>Figure 6.4: Validity Of States In Discretized Environment</figcaption></figure>

### 6.1.4 Search Based Algorithm

After descretizing the environment the next step is to find the path between the current state $S_{Rcurrent}$ and the goal state $S_{Rgoal}$. The simplest approaches to this is using Dijkstra's or the A\* algorithm, which returns the shortest possible path between a given start and goal. However, this can be slow and memory expensive. Instead, a suboptimal solution, or a path from the start to goal that isn't necessarily the best one can be found. This relaxes the conditions on the graph search and allows us to find a good-enough solution in much less time with much less memory. One algorithm that does this is weighted A\*. This relaxation parameter of is often denoted as epsilon of the best solution. When epsilon is set to a value k, this means that the weighted A\* algorithm will return a path that is no worse than $k^{th}$ times the cost of the optimal solution. This concept of suboptimality is fundamental to how search based planners work.

<figure><img src="image_29.png" /><figcaption>Figure 6.5: Dijkstra search algorithm (A) - A\* search algorithm (B) - Weighted A\* Search Algorithm(C)</figcaption></figure>

The figure 6.5 shows a Dijkstra search (sub-figure A), an A\* search (sub-figure B), and a weighted A\* search (sub-figure C). The speed and memory usage in terms of number of nodes that are expanded of each can easily be seen. Dijkstra's is the slowest and expands the most number of states. The A\* is a bit better, and computes an optimal path as well and weighted A\* runs the fastest, but does not return the best solution.

A middle ground solution can be found by using Anytime Repairing. A\* . ARA\* is a search algorithm that combines elements of both A\* search and dynamic programming to efficiently find optimal paths in graphs or state spaces. It is particularly useful when dealing with domains that have large or continuous state spaces. ARA\* works by iteratively refining an initial path using a sequence of decreasing bounds on the path cost. In each iteration, ARA\* performs a series of A\* searches with an increasingly relaxed cost bound until an optimal solution is found. During these searches, ARA\* explores the state space, gradually expanding nodes and updating their cost-to-goal estimates. As the search progresses, the algorithm can terminate at any time, providing a suboptimal solution that improves over time. ARA\* is considered an "anytime" algorithm because it can be interrupted at any point, allowing for trade-offs between solution quality and computation time.
### 6.1.3 State Validity and Collision Checking

One important notion about the state space is the validity of states. When computing a trajectory, we need all states in the trajectory to be valid and avoid any collision.

Figure 6.4 represents the 2D environment where we see that there are states that are invalid. These states are invalid if they coexist with a region defined previously as occupied *O* in the environment. The robot is not allowed to be traveling through them. In this case, those states are obviously invalid. More specifically, the robot would be in collision with the occupied regions. Given that, the robot's logic should take into consideration the prior knowledge about the robot's surrounding environment and a given state and returns a feedback telling whether a collision state occurs or not (or synonymously, if the state is valid).

<figure><img src="image_28.png" /><figcaption>Figure 6.4: Validity Of States In Discretized Environment</figcaption></figure>

### 6.1.4 Search Based Algorithm

After descretizing the environment the next step is to find the path between the current state $S_{Rcurrent}$ and the goal state $S_{Rgoal}$. The simplest approaches to this is using Dijkstra's or the A\* algorithm, which returns the shortest possible path between a given start and goal. However, this can be slow and memory expensive. Instead, a suboptimal solution, or a path from the start to goal that isn't necessarily the best one can be found. This relaxes the conditions on the graph search and allows us to find a good-enough solution in much less time with much less memory. One algorithm that does this is weighted A\*. This relaxation parameter of is often denoted as epsilon of the best solution. When epsilon is set to a value k, this means that the weighted A\* algorithm will return a path that is no worse than $k^{th}$ times the cost of the optimal solution. This concept of suboptimality is fundamental to how search based planners work.

<figure><img src="image_29.png" /><figcaption>Figure 6.5: Dijkstra search algorithm (A) - A\* search algorithm (B) - Weighted A\* Search Algorithm(C)</figcaption></figure>

The figure 6.5 shows a Dijkstra search (sub-figure A), an A\* search (sub-figure B), and a weighted A\* search (sub-figure C). The speed and memory usage in terms of number of nodes that are expanded of each can easily be seen. Dijkstra's is the slowest and expands the most number of states. The A\* is a bit better, and computes an optimal path as well and weighted A\* runs the fastest, but does not return the best solution.

A middle ground solution can be found by using Anytime Repairing. A\* is a search algorithm that combines elements of both A\* search and dynamic programming to efficiently find optimal paths in graphs or state spaces. It is particularly useful when dealing with domains that have large or continuous state spaces. A\* works by iteratively refining an initial path using a sequence of decreasing bounds on the path cost. In each iteration, A\* performs a series of A\* searches with an increasingly relaxed cost bound until an optimal solution is found. During these searches, A\* explores the state space, gradually expanding nodes and updating their cost-to-goal estimates. As the search progresses, the algorithm can terminate at any time, providing a suboptimal solution that improves over time. A\* is considered an "anytime" algorithm because it can be interrupted at any point, allowing for trade-offs between solution quality and computation time.
### 6.1.5 Kinematic Constraints and Motion Primitives

In previous sections, the states of the robot were defined. A trajectory or path as a series of adjacent states in a graph was represented. But adjacent states don't reflect the kinodynamic constraints of the robot. For example, let us consider two adjacent states $S_5$ and $S_7$ shown in figure 6.3. While the states are adjacent, the robot cannot move sideways to achieve this transition or the resulting paths might not be smooth considering a that $\theta$ is changed every step of the path. Instead, lattice-based graphs is used such that they are constructed using motion primitives. Motion primitives are pre-computed motions that the robot can take. By superimposing this on the graph at the robot's position, the adjacent states are not necessarily spatially adjacent. Instead, they represent what the robot can smoothly transition to. The figure 6.6 shows lattice-based graphs for the robot where $C(S_g, S_h)$ is the cost of moving from the state $S_g$ to adjacent state $S_h$ for $S_g$ being the $g^{th}$ state and $S_h$ being the $h^{th}$ state. Valid adjacent states of a given state are the states that can be reached with these motions while not colliding with anything else.

<figure><img src="image_30.png" /><figcaption>Figure 6.6: Motion Primitives [24]</figcaption></figure>

Motion primitives allow the encode of the kinematic constraints of the robot into any environment. They also allow for different costs to be associated with each motion. For example, if the robot is passing through an unfavorable area such as personal spaces as shown in figure 5.6b, a high cost to that particular motion primitive can be assigned. More details about search based algorithms is found in [63].

## 6.2 Model Predictive Control

To follow the trajectory from the global planner and to abide by the constraints a controller is needed. For a dynamic environment a prediction for the motion of the robot in the coming time horizon will help obtaining favorable results. Model predictive control (MPC) is an optimal control technique in which the calculated control actions minimize a cost function for a constrained dynamical system over a finite receding horizon as shown in figure 6.7a. At each time step, an MPC controller receives or estimates the current state of the robot. It then calculates the sequence of control actions as shown in figure 6.7a-b. After that a constrained optimization problem that relies on an internal robot model and depends on the current system state that minimizes the cost over the horizon is solved. Then the controller then applies to the robot only the computed control action, and disregards the other ones as shown in figure 6.7c [42]. A detailed description of the implemented MPC controller depending on the work done by [55] is found in the appendix.

<figure><img src="image_31.png" /><figcaption>Figure 6.7: Model Predictive Control General Operation</figcaption></figure>
### 6.1.5 Kinematic Constraints and Motion Primitives

In previous sections, the states of the robot were defined. A trajectory or path as a series of adjacent states in a graph was represented. But adjacent states don't reflect the kinodynamic constraints of the robot. For example, let us consider two adjacent states $S_5$ and $S_7$ shown in figure 6.3. While the states are adjacent, the robot cannot move sideways to achieve this transition or the resulting paths might not be smooth considering a that $\theta$ is changed every step of the path. Instead, lattice-based graphs is used such that they are constructed using motion primitives. Motion primitives are pre-computed motions that the robot can take. By superimposing this on the graph at the robot's position, the adjacent states are not necessarily spatially adjacent. Instead, they represent what the robot can smoothly transition to. The figure 6.6 shows lattice-based graphs for the robot where $C(S_g, S_h)$ is the cost of moving from the state $S_g$ to adjacent state $S_h$ for $S_g$ being the $g^{th}$ state and $S_h$ being the $h^{th}$ state. Valid adjacent states of a given state are the states that can be reached with these motions while not colliding with anything else.

<figure><img src="image_30.png" /><figcaption>Figure 6.6: Motion Primitives [24]</figcaption></figure>

Motion primitives allow the encode of the kinematic constraints of the robot into any environment. They also allow for different costs to be associated with each motion. For example, if the robot is passing through an unfavorable area such as personal spaces as shown in figure 5.6b, a high cost to that particular motion primitive can be assigned. More details about search based algorithms is found in [63].

## 6.2 Model Predictive Control

To follow the trajectory from the global planner and to abide by the constraints a controller is needed. For a dynamic environment a prediction for the motion of the robot in the coming time horizon will help obtaining favorable results. Model predictive control (MPC) is an optimal control technique in which the calculated control actions minimize a cost function for a constrained dynamical system over a finite receding horizon as shown in figure 6.7a. At each time step, an MPC controller receives or estimates the current state of the robot. It then calculates the sequence of control actions as shown in figure 6.7a-b. After that a constrained optimization problem that relies on an internal robot model and depends on the current system state that minimizes the cost over the horizon is solved. Then the controller then applies to the robot only the computed control action, and disregards the other ones as shown in figure 6.7c [42]. A detailed description of the implemented MPC controller depending on the work done by [55] is found in the appendix.

<figure><img src="image_31.png" /><figcaption>Figure 6.7: Model Predictive Control General Operation</figcaption></figure>
# Simulation and Results

In order to verify the usefulness and effectiveness of the proposed hierarchical planner, we have implemented and tested it in simulation environments. The software of the proposed framework was implemented using the C/C++ and Python programming language. The entire navigation framework was developed on the Robot Operating System (ROS) [49] and visualized in RViz – the 3D visualization tool for ROS. The conventional search based algorithms, lattice graph generator and model predictive control planner packages were inherited and modified for developing the proposed hierarchical planner.

## 7.1 Simulation Setup

we created and conducted different scenarios and experiments in gazebo simulation environment which is an open source simulator used for ROS and visualized the results on RViz. Figure 7.1 illustrates the scenario of manual handling between a robot and a human handler without the presence of any other humans in the loading area and figures 7.2 and 7.3 illustrates another scenario were the handler is performing a forward passing and cross passing scenarios in the environment respectively. In order to validate the proposed planner the robot should succeed in accomplishing a handling operation. This is evaluated by achieving a collision free mission, reaching the passing region that abides by ergonomic of manual handling, abiding by speed limit, and respecting social constraints such as personal spaces.

### 7.1.1 Loading Scenario

In the below loading scenario the robot is required to perform a manual handling operation to the handler by moving from its start point denoted by A to a goal point in the passing region denoted by B as shown in figure 7.1. In this figure we can see the representation of the passing region described in section 5.5.2 Where the robot’s

<figure><img src="image_32.png" /><figcaption>(a) Handling operation scenario for a robot traveling from point A to B following the generated path and achieving handling operation in the passing region of handler H while respecting social spaces around him</figcaption></figure>

<figure><img src="image_33.png" /><figcaption>(b) Comfort speed of the robot during handling operation</figcaption></figure>

<caption>Figure 7.1: Handling operation and Comfort speed of the Robot</caption>

target goal is inside the personal space but not violating the social norms due to the nature of task the robot is
# Simulation and Results

In order to verify the usefulness and effectiveness of the proposed hierarchical planner, we have implemented and tested it in simulation environments. The software of the proposed framework was implemented using the C/C++ and Python programming language. The entire navigation framework was developed on the Robot Operating System (ROS) [49] and visualized in RViz – the 3D visualization tool for ROS. The conventional search based algorithms, lattice graph generator and model predictive control planner packages were inherited and modified for developing the proposed hierarchical planner.

## 7.1 Simulation Setup

we created and conducted different scenarios and experiments in gazebo simulation environment which is an open source simulator used for ROS and visualized the results on RViz. Figure 7.1 illustrates the scenario of manual handling between a robot and a human handler without the presence of any other humans in the loading area and figures 7.2 and 7.3 illustrates another scenario where the handler is performing a forward passing and cross passing scenarios in the environment respectively. In order to validate the proposed planner the robot should succeed in accomplishing a handling operation. This is evaluated by achieving a collision free mission, reaching the passing region that abides by ergonomic of manual handling, abiding by speed limit, and respecting social constraints such as personal spaces.

### 7.1.1 Loading Scenario

In the below loading scenario the robot is required to perform a manual handling operation to the handler by moving from its start point denoted by A to a goal point in the passing region denoted by B as shown in figure 7.1. In this figure we can see the representation of the passing region described in section 5.5.2 Where the robot’s

<figure><img src="image_32.png" /><figcaption>(a) Handling operation scenario for a robot traveling from point A to B following the generated path and achieving handling operation in the passing region of handler H while respecting social spaces around him</figcaption></figure>

<figure><img src="image_33.png" /><figcaption>(b) Comfort speed of the robot during handling operation</figcaption></figure>

<caption>Figure 7.1: Handling operation and Comfort speed of the Robot</caption>

target goal is inside the personal space but not violating the social norms due to the nature of task the robot is
doing which is manual handling operation. Moreover we can see the effect of the social space around the handler *H* represented in a form of costs on the 2d costmap. Moreover, the speed of the robot is constrained along with the handling operation to the comfort speed *V*<sub>comfort</sub>. As shown in figure 7.1b the robot's speed is being upper bounded by the comfort speed. Finally, by applying *ARA*\* as a sampling based search algorithm a sub-optimal path from the current state to the goal state is guaranteed. This complies with minimizing time of operation constraint given from the logistics.

### 7.1.2 Forward Passing Scenario

In the second scenario shown in 7.2 represents the robot performing a handling operation for a handler that is directing towards the robot. It is called forward passing scenario. The dynamic handler is represented by a green cylinder as discussed in chapter 5 and the red dot represents the best handling point available in the handling region discussed in section 5.5.2 where the robot is assigned to navigate to. Since we are using a sampling based algorithm checking its behavior upon changing the goal position and its ability to accommodate with this change is essential. Each number represents the state of the robot and the handler at a certain time instance where 1 represents their state at the first time instance and 3 represents their state at the third time instance. Figure 7.2 shows that the robot with the suggested algorithm was able to adapt its path upon changing the goal and perform a successful handling operation upon reaching the handler at time instance 3.

<figure><img src="image_34.png" /><figcaption>Figure 7.2: Forward Passing Scenario</figcaption></figure>

### 7.1.3 Cross Passing Scenario

In the third scenario shown in 7.3 represents the robot performing a handling operation for a handler that is crossing in-front of the robot and stays walking for a certain distance then stops. It is called cross passing scenario. Figure 7.3 shows that the robot with the suggested algorithm was able to adapt its path upon changing the goal and perform a successful handling operation when the operator stops in the end of the operation.
doing which is manual handling operation. Moreover we can see the effect of the social space around the handler *H* represented in a form of costs on the 2d costmap. Moreover, the speed of the robot is constrained along with the handling operation to the comfort speed *V*<sub>comfort</sub>. As shown in figure 7.1b the robot's speed is being upper bounded by the comfort speed. Finally, by applying *ARA*\* as a sampling based search algorithm a sub-optimal path from the current state to the goal state is guaranteed. This complies with minimizing time of operation constraint given from the logistics.

### 7.1.2 Forward Passing Scenario

In the second scenario shown in 7.2 represents the robot performing a handling operation for a handler that is directing towards the robot. It is called forward passing scenario. The dynamic handler is represented by a green cylinder as discussed in chapter 5 and the red dot represents the best handling point available in the handling region discussed in section 5.5.2 where the robot is assigned to navigate to. Since we are using a sampling based algorithm checking its behavior upon changing the goal position and its ability to accommodate with this change is essential. Each number represents the state of the robot and the handler at a certain time instance where 1 represents their state at the first time instance and 3 represents their state at the third time instance. Figure 7.2 shows that the robot with the suggested algorithm was able to adapt its path upon changing the goal and perform a successful handling operation upon reaching the handler at time instance 3.

<figure><img src="image_34.png" /><figcaption>Figure 7.2: Forward Passing Scenario</figcaption></figure>

### 7.1.3 Cross Passing Scenario

In the third scenario shown in 7.3 represents the robot performing a handling operation for a handler that is crossing in-front of the robot and stays walking for a certain distance then stops. It is called cross passing scenario. Figure 7.3 shows that the robot with the suggested algorithm was able to adapt its path upon changing the goal and perform a successful handling operation when the operator stops in the end of the operation.
<figure><img src="image_35.png" /><figcaption>Figure 7.3: Robot's Performance In a Cross Passing Scenario</figcaption></figure>

## 7.2 Conclusion

During the internship, several tasks were undertaken. Firstly, an extensive review of the current state of the art and previous research in the field of intralogistics was conducted. This provided valuable insights into existing work and established a foundation for the project. Additionally, a deep exploration of various disciplines in logistics, social science, and robotics was carried out to identify a problem that could contribute to optimizing handover operations. The aim was to define a relevant problem that addressed the specific challenges of loading-unloading and transfer operations separately. Subsequently, all the pertinent constraints related to handover operations were identified, taking into account the factors from logistics, social science, and robotics. This comprehensive understanding of the constraints formed the basis for the subsequent stages. Another significant achievement was the development of a simulation environment that served as a testing platform for future work. This platform enabled the evaluation and validation of different approaches and strategies in a controlled setting.

Finally, a suggested hierarchical planner was implemented and tested, specifically designed to accommodate the identified constraints. The testing and validation process provided valuable insights into the effectiveness and feasibility of the planner in optimizing handover operations. Overall, these tasks collectively contributed to a comprehensive exploration of the domain, definition of relevant problems, development of a simulation environment, and evaluation of a suggested hierarchical planner, all aimed at improving handover operations. To assess the effectiveness and practicality of the proposed hierarchical planner, extensive testing was conducted within simulation environments. The software framework of the planner was implemented using C/C++ and Python, integrated with the Robot Operating System (ROS) and visualized using RViz. Various conventional search-based algorithms, lattice graph generators, and model predictive control planner packages were adapted to develop the hierarchical planner. In the simulation setup, multiple scenarios were created and evaluated using the Gazebo simulation environment, which is specifically designed for ROS. The experiments included a manual handling scenario between a robot and a human handler, where the robot successfully navigated from a start point to a goal point in a passing region, while considering social spaces and adhering to a comfort speed. Additionally, scenarios involving forward passing and cross passing were performed, demonstrating the planner's ability to adapt paths and achieve successful handling operations. The simulations showcased the planner's practicality in real-world situations and its capability to fulfill mission requirements, such as collision-free operation, ergonomic adherence, speed limits, and respect for social constraints. The demos for the test of the previously suggested scenarios are presented in [21].
<figure><img src="image_35.png" /><figcaption>Figure 7.3: Robot's Performance In a Cross Passing Scenario</figcaption></figure>

## 7.2 Conclusion

During the internship, several tasks were undertaken. Firstly, an extensive review of the current state of the art and previous research in the field of intralogistics was conducted. This provided valuable insights into existing work and established a foundation for the project. Additionally, a deep exploration of various disciplines in logistics, social science, and robotics was carried out to identify a problem that could contribute to optimizing handover operations. The aim was to define a relevant problem that addressed the specific challenges of loading-unloading and transfer operations separately. Subsequently, all the pertinent constraints related to handover operations were identified, taking into account the factors from logistics, social science, and robotics. This comprehensive understanding of the constraints formed the basis for the subsequent stages. Another significant achievement was the development of a simulation environment that served as a testing platform for future work. This platform enabled the evaluation and validation of different approaches and strategies in a controlled setting.

Finally, a suggested hierarchical planner was implemented and tested, specifically designed to accommodate the identified constraints. The testing and validation process provided valuable insights into the effectiveness and feasibility of the planner in optimizing handover operations. Overall, these tasks collectively contributed to a comprehensive exploration of the domain, definition of relevant problems, development of a simulation environment, and evaluation of a suggested hierarchical planner, all aimed at improving handover operations. To assess the effectiveness and practicality of the proposed hierarchical planner, extensive testing was conducted within simulation environments. The software framework of the planner was implemented using C/C++ and Python, integrated with the Robot Operating System (ROS) and visualized using RViz. Various conventional search-based algorithms, lattice graph generators, and model predictive control planner packages were adapted to develop the hierarchical planner. In the simulation setup, multiple scenarios were created and evaluated using the Gazebo simulation environment, which is specifically designed for ROS. The experiments included a manual handling scenario between a robot and a human handler, where the robot successfully navigated from a start point to a goal point in a passing region, while considering social spaces and adhering to a comfort speed. Additionally, scenarios involving forward passing and cross passing were performed, demonstrating the planner's ability to adapt paths and achieve successful handling operations. The simulations showcased the planner's practicality in real-world situations and its capability to fulfill mission requirements, such as collision-free operation, ergonomic adherence, speed limits, and respect for social constraints. The demos for the test of the previously suggested scenarios are presented in [21].
# A

# Appendix

## A.1 Local Planner

### A.1.1 Model Predictive Controller

In this section the local planner used will be briefly described. This local planner is based on the model predictive control approach used in [55]. We will start by describing the motion planning problem tackled in this approach.

### A.1.2 Motion Planning Problem Description

This approach addresses robotic systems described by nonlinear and time-invariant differential equations with time $t \in R$, state trajectory $x: R \to X$ and control trajectory $u: R \to U$ as mentioned in the loading problem specification part where $\dot{x}(t) = f(x(t), u(t))$.

The mapping $f: X \times U \to R$ with $p = dim(X)$ is continuous. The state space $X$ and control space $U$ do not have to be exclusively Euclidean spaces, as discussed later. This is subject to state and input constraints originating from internal robot constraints, but also from the dynamic environment in which the robot operates. Therefore, state constraints $x(t) \in X(t) \subset X$ and input constraints $u(t) \in U(x(t), t) \subset U$ are time variant.

### A.1.3 Local Planning via Optimal Control

The planning task is to guide this robotic system from $\mathbf{x}_s \in \mathcal{X}$ at time $t_s$ to an intermediate or ultimate goal set $X_f(t_s) \subset \mathcal{X}$ within time $t_f \in I$ while minimizing an objective function and adhering to constraints. Hereby, $I \subseteq R$ denotes a predefined time interval containing $t_s$. With running cost $\ell: \mathcal{X} \times \mathcal{U} \mapsto R_0^+$ and terminal cost $J_f: \mathcal{X} \mapsto R_0^+$, the OCP is given as follows:

$$ \min_{\mathbf{u}(t), \mathbf{x}(t), t_f} J_f(\mathbf{x}(t_f)) + \int_{t=t_s}^{t_f} \ell(\mathbf{x}(t), \mathbf{u}(t))dt $$

subject to

$$ \mathbf{x}(t_s) = \mathbf{x}_s, \quad \dot{\mathbf{x}}(t) = \mathbf{f}(\mathbf{x}(t), \mathbf{u}(t)), \quad \mathbf{x}(t_f) \in X_f(t_s), $$

$$ \mathbf{x}(t) \in X(t), \quad \mathbf{u}(t) \in U(\mathbf{x}(t), t), \quad \dot{\mathbf{u}}(t) \in U_d, t_f \in I. $$

State and control constraints are included as described before. In addition, the control derivative $\dot{u}(t)$ is restricted to a given set $U_d$. Note, the same result can be achieved by augmenting the state space with integrator dynamics. However, in robot applications the system is often described by a kinematic model with velocities as input, and therefore such a constraint (i.e. acceleration limits) eliminates the need to increase the number of optimization parameters. OCP is generic at this point and includes the most common objectives like minimizing time or minimizing control error and effort (quadratic form). These are detailed in numerical realization Section. Note that potential fields for increasing the distances to obstacles or attraction terms for approaching way points may also be included in $\ell(\cdot)$.

### A.1.4 Feedback Control

An MPC-based local planner provides control actions directly to the robot or to a cascaded low-level controller. In practice, OCP can only be solved at discrete time instances and hence the control law is defined according to the grid $t_0 < t_1 < \dots < t_n < \dots < \infty$ with $n \in N_0$ and $t_n \in R_0^+$. The control law $\mu: \mathcal{X} \mapsto \mathcal{U}$ for $t \in [t_n, t_{n+1})$ is given by:
# A

# Appendix

## A.1 Local Planner

### A.1.1 Model Predictive Controller

In this section the local planner used will be briefly described. This local planner is based on the model predictive control approach used in [55]. We will start by describing the motion planning problem tackled in this approach.

### A.1.2 Motion Planning Problem Description

This approach addresses robotic systems described by nonlinear and time-invariant differential equations with time $t \in R$, state trajectory $x: R \to X$ and control trajectory $u: R \to U$ as mentioned in the loading problem specification part where $\dot{x}(t) = f(x(t), u(t))$.

The mapping $f: X \times U \to R$ with $p = dim(X)$ is continuous. The state space $X$ and control space $U$ do not have to be exclusively Euclidean spaces, as discussed later. This is subject to state and input constraints originating from internal robot constraints, but also from the dynamic environment in which the robot operates. Therefore, state constraints $x(t) \in X(t) \subset X$ and input constraints $u(t) \in U(x(t), t) \subset U$ are time variant.

### A.1.3 Local Planning via Optimal Control

The planning task is to guide this robotic system from $\mathbf{x}_s \in \mathcal{X}$ at time $t_s$ to an intermediate or ultimate goal set $X_f(t_s) \subset \mathcal{X}$ within time $t_f \in I$ while minimizing an objective function and adhering to constraints. Hereby, $I \subseteq R$ denotes a predefined time interval containing $t_s$. With running cost $\ell: \mathcal{X} \times \mathcal{U} \mapsto R_0^+$ and terminal cost $J_f: \mathcal{X} \mapsto R_0^+$, the OCP is given as follows:

$$ \min_{\mathbf{u}(t), \mathbf{x}(t), t_f} J_f(\mathbf{x}(t_f)) + \int_{t=t_s}^{t_f} \ell(\mathbf{x}(t), \mathbf{u}(t))dt $$

subject to

$$ \mathbf{x}(t_s) = \mathbf{x}_s, \quad \dot{\mathbf{x}}(t) = \mathbf{f}(\mathbf{x}(t), \mathbf{u}(t)), \quad \mathbf{x}(t_f) \in X_f(t_s), $$

$$ \mathbf{x}(t) \in X(t), \quad \mathbf{u}(t) \in U(\mathbf{x}(t), t), \quad \dot{\mathbf{u}}(t) \in U_d, t_f \in I. $$

State and control constraints are included as described before. In addition, the control derivative $\dot{u}(t)$ is restricted to a given set $U_d$. Note, the same result can be achieved by augmenting the state space with integrator dynamics. However, in robot applications the system is often described by a kinematic model with velocities as input, and therefore such a constraint (i.e. acceleration limits) eliminates the need to increase the number of optimization parameters. OCP is generic at this point and includes the most common objectives like minimizing time or minimizing control error and effort (quadratic form). These are detailed in numerical realization Section. Note that potential fields for increasing the distances to obstacles or attraction terms for approaching way points may also be included in $\ell(\cdot)$.

### A.1.4 Feedback Control

An MPC-based local planner provides control actions directly to the robot or to a cascaded low-level controller. In practice, OCP can only be solved at discrete time instances and hence the control law is defined according to the grid $t_0 < t_1 < \dots < t_n < \dots < \infty$ with $n \in N_0$ and $t_n \in R_0^+$. The control law $\mu: \mathcal{X} \mapsto \mathcal{U}$ for $t \in [t_n, t_{n+1})$ is given by:
$$ \mu(\mathbf{x}(t)) := \mathbf{u}^*(t)|_{\mathbf{x}_s=\mathbf{x}(t_n), t_s=t_n} $$

Hereby, $\mathbf{u}^*(t)$ denotes the resulting optimal control trajectory from OCP with substitutions $\mathbf{x}_s = \mathbf{x}(t_n)$ and $t_s = t_n$. The current state $\mathbf{x}(t_n)$ is either directly measurable or estimated by a state observer.

### A.1.5 Numerical Realization

This section transforms the generic motion planning problem into a numerically tractable realization.

#### Alternative State Spaces

Common MPC formulations usually assume that the state space is Euclidean (resp. the real n-space). But especially in robotics, state spaces often include rotational components which span over the non-Euclidean rotation groups. A possible approach is to provide an over-parameterized formulation with constraints. However, local derivative-based optimization schemes apply local increments $\mathbf{z}^* = \mathbf{z} + \Delta\mathbf{z}$ with parameter $\mathbf{z} \in \mathbb{R}^M$ and increment $\Delta\mathbf{z} \in \mathbb{R}^M$ over an M-dimensional Euclidean parameter space in each solver iteration [21] without maintaining any of these difficulties. To overcome these difficulties, we adapt and extend the idea of alternative parameterizations from graph-based SLAM [55], which we have already successfully applied in simplified form in the time elastic band implementation. The idea follows the observation that $\Delta\mathbf{z}$ is usually a small perturbation around $\mathbf{z}$ and hence far from singularities. Therefore, $\Delta\mathbf{z}$ represents a minimal representation computed in the local Euclidean surroundings of $\mathbf{z}$. A nonlinear increment operator then converts and applies $\Delta\mathbf{z}$ to the correct space. Due to the limited scope of this work, we restrict ourselves to 2D rotation groups which applies to our robot navigation scenario. In this case, even $\mathbf{z}$ does not need to be over-parameterized but the nonlinear increment operator still maintains proper 2D rotations.

Consider a state $\mathbf{x} \in \mathcal{X}$. An increment $\Delta\mathbf{x} \in \mathbb{R}^p$ is then applied by the nonlinear increment operator $\oplus : \mathcal{X} \times \mathbb{R}^p \mapsto \mathcal{X}$:

$$ \mathbf{x}^* = \mathbf{x} \oplus \Delta\mathbf{x} $$

Hereby, $\mathbb{R}^p$ is a local Euclidean space. To properly evaluate the system dynamics in the alternative state space, the difference $\mathbf{x}_2 - \mathbf{x}_1$ for $\mathbf{x}_1, \mathbf{x}_2 \in \mathcal{X}$ must be embedded into $\mathcal{X}$. This is achieved by defining a nonlinear difference operator $\ominus: \mathcal{X} \times \mathcal{X} \mapsto \mathcal{X}$ such that $\delta\mathbf{x} = \mathbf{x}_2 \ominus \mathbf{x}_1$. Note that no singularities occur for rotational components in $SO(2)$ and the operator defines the shortest angular distance between two states without discontinuities. Here we apply these operators only to states, but the extension to alternative control input spaces is done in exactly the same way.

#### Direct Transcription

This section applies direct transcription in combination with the previously defined increment and difference operators to convert OCP to an NLP. The time interval $[t_s, t_f]$ of the planning horizon is now discretized according to the following grid: $t_s = t_0 \le t_1 \le \dots \le t_k \le \dots \le t_N = t_f$ with $t_k \in I, k = 0, 1, \dots, N$ and $N \in \mathbb{N}$. Note that index $k$ indicates that the context belongs to prediction rather than closed-loop control. Furthermore, $\Delta t_k = t_{k+1} - t_k$ denotes the time interval for an individual grid partition. Here, direct transcription relies on a piece-wise constant control trajectory with respect to the temporal grid, i.e. $\mathbf{u}(t) := \mathbf{u}_k = \text{constant for } t \in [t_k, t_k + \Delta t_k)$ for $k = 0, 1, \dots, N-1$. The states at grid points $t_k$ are denoted as $\mathbf{x}(t_k) := \mathbf{x}_k$ for $k = 0, 1, \dots, N$.

Several methods exist to discretize the boundary value problem in OCP induced by system [37]. In the following, we utilize low order collocation via finite differences as they provide a reasonable trade-off between computational resources and accuracy and they are directly suitable for the nonlinear difference operator $\ominus$.

The system dynamics error on the $k^{th}$ grid partition is approximated by a basis function $\phi(\mathbf{x}_{k+1}, \mathbf{x}_k, \mathbf{u}_k, \Delta t_k) = (\mathbf{x}_{k+1} \ominus \mathbf{x}_k) \Delta t_k^{-1} - \xi(\mathbf{x}_{k+1}, \mathbf{x}_k, \mathbf{u}_k)$ with a suitable finite difference kernel $\xi(\cdot)$, i.e. either forward differences (explicit, first order, similar to multiple shooting with forward Euler and full discretization):

$$ \xi(\mathbf{x}_{k+1}, \mathbf{x}_k, \mathbf{u}_k) := \mathbf{f}(\mathbf{x}_k, \mathbf{u}_k) $$

or Crank-Nicolson differences (implicit, second order):

$$ \xi(\mathbf{x}_{k+1}, \mathbf{x}_k, \mathbf{u}_k) := 0.5 (\mathbf{f}(\mathbf{x}_k, \mathbf{u}_k) + \mathbf{f}(\mathbf{x}_{k+1}, \mathbf{u}_k)). $$
$$ \mu(\mathbf{x}(t)) := \mathbf{u}^*(t)|_{\mathbf{x}_s=\mathbf{x}(t_s), t_s=t_n} $$

Hereby, $\mathbf{u}^*(t)$ denotes the resulting optimal control trajectory from OCP with substitutions $\mathbf{x}_s = \mathbf{x}(t_n)$ and $t_s = t_n$. The current state $\mathbf{x}(t_n)$ is either directly measurable or estimated by a state observer.

### A.1.5 Numerical Realization

This section transforms the generic motion planning problem into a numerically tractable realization.

#### Alternative State Spaces

Common MPC formulations usually assume that the state space is Euclidean (resp. the real $n$-space). But especially in robotics, state spaces often include rotational components which span over the non-Euclidean rotation groups. A possible approach is to provide an over-parameterized formulation with constraints. However, local derivative-based optimization schemes apply local increments $\mathbf{z}^* = \mathbf{z} + \Delta\mathbf{z}$ with parameter $\mathbf{z} \in \mathbb{R}^M$ and increment $\Delta\mathbf{z} \in \mathbb{R}^M$ over an $M$-dimensional Euclidean parameter space in each solver iteration [21] without maintaining any of these difficulties. To overcome these difficulties, we adapt and extend the idea of alternative parameterizations from graph-based SLAM [55], which we have already successfully applied in simplified form in the time elastic band implementation. The idea follows the observation that $\Delta\mathbf{z}$ is usually a small perturbation around $\mathbf{z}$ and hence far from singularities. Therefore, $\Delta\mathbf{z}$ represents a minimal representation computed in the local Euclidean surroundings of $\mathbf{z}$. A nonlinear increment operator then converts and applies $\Delta\mathbf{z}$ to the correct space. Due to the limited scope of this work, we restrict ourselves to 2D rotation groups which applies to our robot navigation scenario. In this case, even $\mathbf{z}$ does not need to be over-parameterized but the nonlinear increment operator still maintains proper 2D rotations.

Consider a state $\mathbf{x} \in \mathcal{X}$. An increment $\Delta\mathbf{x} \in \mathbb{R}^p$ is then applied by the nonlinear increment operator $\oplus : \mathcal{X} \times \mathbb{R}^p \mapsto \mathcal{X}$:

$$ \mathbf{x}^* = \mathbf{x} \oplus \Delta\mathbf{x} $$

Hereby, $\mathbb{R}^p$ is a local Euclidean space. To properly evaluate the system dynamics in the alternative state space, the difference $\mathbf{x}_2 - \mathbf{x}_1$ for $\mathbf{x}_1, \mathbf{x}_2 \in \mathcal{X}$ must be embedded into $\mathcal{X}$. This is achieved by defining a nonlinear difference operator $\ominus: \mathcal{X} \times \mathcal{X} \mapsto \mathcal{X}$ such that $\delta\mathbf{x} = \mathbf{x}_2 \ominus \mathbf{x}_1$. Note that no singularities occur for rotational components in $SO(2)$ and the operator defines the shortest angular distance between two states without discontinuities. Here we apply these operators only to states, but the extension to alternative control input spaces is done in exactly the same way.

#### Direct Transcription

This section applies direct transcription in combination with the previously defined increment and difference operators to convert OCP to an NLP. The time interval $[t_s, t_f]$ of the planning horizon is now discretized according to the following grid: $t_s = t_0 \le t_1 \le \dots \le t_k \le \dots \le t_N = t_f$ with $t_k \in I, k = 0, 1, \dots, N$ and $N \in \mathbb{N}$. Note that index $k$ indicates that the context belongs to prediction rather than closed-loop control. Furthermore, $\Delta t_k = t_{k+1} - t_k$ denotes the time interval for an individual grid partition. Here, direct transcription relies on a piece-wise constant control trajectory with respect to the temporal grid, i.e. $\mathbf{u}(t) := \mathbf{u}_k = \text{constant for } t \in [t_k, t_k + \Delta t_k)$ for $k = 0, 1, \dots, N-1$. The states at grid points $t_k$ are denoted as $\mathbf{x}(t_k) := \mathbf{x}_k$ for $k = 0, 1, \dots, N$.

Several methods exist to discretize the boundary value problem in OCP induced by system [37]. In the following, we utilize low order collocation via finite differences as they provide a reasonable trade-off between computational resources and accuracy and they are directly suitable for the nonlinear difference operator $\ominus$.

The system dynamics error on the $k^{th}$ grid partition is approximated by a basis function $\phi(\mathbf{x}_{k+1}, \mathbf{x}_k, \mathbf{u}_k, \Delta t_k) = (\mathbf{x}_{k+1} \ominus \mathbf{x}_k) \Delta t_k^{-1} - \xi(\mathbf{x}_{k+1}, \mathbf{x}_k, \mathbf{u}_k)$ with a suitable finite difference kernel $\xi(\cdot)$, i.e. either forward differences (explicit, first order, similar to multiple shooting with forward Euler and full discretization):

$$ \xi(\mathbf{x}_{k+1}, \mathbf{x}_k, \mathbf{u}_k) := \mathbf{f}(\mathbf{x}_k, \mathbf{u}_k) $$

or Crank-Nicolson differences (implicit, second order):

$$ \xi(\mathbf{x}_{k+1}, \mathbf{x}_k, \mathbf{u}_k) := 0.5 (\mathbf{f}(\mathbf{x}_k, \mathbf{u}_k) + \mathbf{f}(\mathbf{x}_{k+1}, \mathbf{u}_k)). $$
By further approximating the integral cost in OCP by the right Riemann sum and $\dot{u}(t)$ by forward differences, the resulting NLP is given as follows:

$$ \min_{\mathbf{u}_0, \mathbf{x}_1, \dots, \mathbf{u}_{N-1}, \Delta\mathbf{x}_0, \mathbf{x}_1, \dots, \mathbf{x}_N, \Delta t_0, \Delta t_1, \dots, \Delta t_{N-1}} J_f(\mathbf{x}_N) + \sum_{k=0}^{N-1} \ell(\mathbf{x}_k, \mathbf{u}_k) \Delta t_k $$

subject to

$$ \begin{array}{l} \mathbf{x}_0 = \mathbf{x}_s, \mathbf{x}_N \in \mathbb{X}_f(t_s), \mathbf{x}_k \in \mathbb{X}(k\Delta t_k), \mathbf{u}_k \in \mathbb{U}(\mathbf{x}_k, k\Delta t_k), (\mathbf{u}_{k+1} - \mathbf{u}_k)\Delta t_k^{-1} \in \mathbb{U}_d, (\mathbf{u}_0 - \mathbf{u}_p)\Delta t_p^{-1} \in \mathbb{U}_d, \\ \Delta t_{\min} \le \Delta t_0 \le \Delta t_{\max}, \quad \Delta t_k = \Delta t_{k+1}, \\ \phi(\mathbf{x}_{k+1}, \mathbf{x}_k, \mathbf{u}_k, \Delta t_k) = \mathbf{0}, \quad k = 0, 1, \dots, N-1. \end{array} $$

The limitation of the first control w.r.t. its derivative at $t_0$ requires the specification of the previous control input $\mathbf{u}_p \in \mathcal{U}$ and the time elapsed since $\mathbf{u}_p$, i.e. $\Delta t_p = t_n - t_{n-1}$. In the very first closed-loop step, $\mathbf{u}_p$ is set to zero or some control for which $f(\mathbf{x}_s, \mathbf{u}_p) = \mathbf{0}$ holds. The same holds for the last control $\mathbf{u}_N$ which is not subject to optimization but provides the final boundary value for the control derivative. This way, a robot with velocity input always plans to stop at the end of the horizon for safety reasons. Note that this is not conventional in receding-horizon MPC as this inherently leads to differing open-loop and closed-loop solutions and might affect convergence to $\mathbb{X}_f(t_s)$. However, with terminal equality conditions as in the time-optimal variant shown later, this does not lead to changes in terms of convergence due to the optimality principle. Condition $t_f \in I$ in OCP has been replaced by $\Delta t_{\min} \le \Delta t_0 \le \Delta t_{\max}$ with bounds $\Delta t_{\min}, \Delta t_{\max} \in \mathbb{R}_+^0$. Furthermore, condition $\Delta t_k = \Delta t_{k+1}$ ensures uniformity between all time intervals. Notice that for the set evaluations $t_k = k\Delta t_0 = k\Delta t_k$ holds due to the uniform grid and choosing $k\Delta t_k$ preserves the local structure of the optimization problem.

## Receding-Horizon Quadratic-Form MPC

The most common MPC realization utilizes a quadratic form objective to minimize the quadratic control error and effort. But this metric must be adjusted to account for alternative state space representations. Common metrics for rotation groups are provided in [37], but here we include the nonlinear difference operator in the quadratic form:

$$ J_f(\mathbf{x}_N) = (\mathbf{x}_N \boxminus \mathbf{x}_f)^\top \mathbf{Q}_f (\mathbf{x}_N \boxminus \mathbf{x}_f), \\ \ell(\mathbf{x}_k, \mathbf{u}_k) = (\mathbf{x}_k \boxminus \mathbf{x}_f)^\top \mathbf{Q} (\mathbf{x}_k \boxminus \mathbf{x}_f) + \mathbf{u}_k^\top \mathbf{R} \mathbf{u}_k. $$

Hereby, $\mathbf{Q}, \mathbf{Q}_f \in \mathbb{R}^{p \times p}$ and $\mathbf{R} \in \mathbb{R}^{q \times q}$ denote weighting matrices with state dimension $p = \dim(\mathcal{X}')$ and input dimension $q = \dim(\mathcal{U})$, respectively. Furthermore, $\mathbf{x}_f \in \mathbb{X}_f(t_n)$ represents the current intermediate goal state. In case the full goal state is not available, it is possible to either choose additional states to complete a steady state or to set related components in $\mathbf{Q}$ and $\mathbf{Q}_f$ to zero. Note that $\mathbf{x}_f$ could also be replaced by a time-dependent reference $\mathbf{x}_f(t)$ during optimization for tracking control. In this MPC realization, the horizon length resp. final time $t_f$ is fixed to $I = \{N\Delta t_s\}$ which implies $\Delta t_{\min} = \Delta t_{\max} = \Delta t_k = \Delta t_s$ with a predefined grid resolution $\Delta t_s \in \mathbb{R}^+$. Therefore the optimization w.r.t. time becomes obsolete and thus the optimization parameters $\Delta t_k$ with $k = 0, 1, \dots, N-1$ as well as temporal constraints are excluded to reduce the dimensions of the optimization problem.

Stability and recursive feasibility conditions for MPC usually rely on proper terminal conditions $J_f(\cdot)$ resp. $\mathbf{Q}_f$ and $\mathbb{X}_f(t_n)$ to, e.g., approximate an infinite horizon. Notice that a sampled data discrete-time representation follows by setting $\Delta t_s = 1$. For (output) path-following MPC, [9] provides terminal conditions and conditions on the reference path under which stability is guaranteed. However, as a suitable terminal region is difficult to determine, most practical approaches usually relay on sufficiently long horizon lengths to ensure stability and feasibility and choose $\mathbb{X}_f(t_n) := \mathcal{X}$. For static environments and a class of robotic systems, i.e. differential drive robots, [54] guarantees stability for shorter receding horizons.

## Obstacle Avoidance Constraints

Here, obstacles are defined as simply connected regions $\mathcal{O}_l(t), l = 1, \dots, L$ with $L$ as the number of obstacles. The time-dependency explicitly accounts for dynamic obstacles. Furthermore, let $\mathcal{R}(\mathbf{x})$ denote a geometric robot collision model dependent on the state $\mathbf{x} \in \mathcal{X}$, e.g. position and orientation. The minimum distance $d_l: \mathcal{X} \times \mathbb{R} \mapsto \mathbb{R}_+^0$ between the robot and obstacle $l$ is then determined by:

$$ d_l(\mathbf{x}, t) = \min_{\mathbf{p}_1 \in \mathcal{R}(\mathbf{x}), \mathbf{p}_2 \in \mathcal{O}_l(t)} \|\mathbf{p}_2 - \mathbf{p}_1\|_2. $$

By specifying a minimum separation to obstacles $d_{\min} \in \mathbb{R}_+^0$, the collision-free state space $\mathbb{X}_0(t)$ is given as follows:
By further approximating the integral cost in OCP by the right Riemann sum and $\dot{u}(t)$ by forward differences, the resulting NLP is given as follows:

$$ \min_{\mathbf{u}_0, \mathbf{x}_1, \dots, \mathbf{u}_{N-1}, \Delta\mathbf{x}_0, \mathbf{x}_1, \dots, \mathbf{x}_N, \Delta t_0, \Delta t_1, \dots, \Delta t_{N-1}} J_f(\mathbf{x}_N) + \sum_{k=0}^{N-1} \ell(\mathbf{x}_k, \mathbf{u}_k) \Delta t_k $$

subject to

$$ \mathbf{x}_0 = \mathbf{x}_s, \mathbf{x}_N \in \mathbb{X}_f(t_s), \mathbf{x}_k \in \mathbb{X}(k\Delta t_k), \mathbf{u}_k \in \mathbb{U}(\mathbf{x}_k, k\Delta t_k), (\mathbf{u}_{k+1} - \mathbf{u}_k)\Delta t_k^{-1} \in \mathbb{U}_d, (\mathbf{u}_0 - \mathbf{u}_p)\Delta t_p^{-1} \in \mathbb{U}_d $$

$$ \Delta t_{\min} \le \Delta t_0 \le \Delta t_{\max}, \quad \Delta t_k = \Delta t_{k+1}, $$

$$ \phi(\mathbf{x}_{k+1}, \mathbf{x}_k, \mathbf{u}_k, \Delta t_k) = \mathbf{0}, \quad k = 0, 1, \dots, N-1. $$

The limitation of the first control w.r.t. its derivative at $t_0$ requires the specification of the previous control input $\mathbf{u}_p \in \mathcal{U}$ and the time elapsed since $\mathbf{u}_p$, i.e. $\Delta t_p = t_n - t_{n-1}$. In the very first closed-loop step, $\mathbf{u}_p$ is set to zero or some control for which $f(\mathbf{x}_s, \mathbf{u}_p) = \mathbf{0}$ holds. The same holds for the last control $\mathbf{u}_N$ which is not subject to optimization but provides the final boundary value for the control derivative. This way, a robot with velocity input always plans to stop at the end of the horizon for safety reasons. Note that this is not conventional in receding-horizon MPC as this inherently leads to differing open-loop and closed-loop solutions and might affect convergence to $\mathbb{X}_f(t_s)$. However, with terminal equality conditions as in the time-optimal variant shown later, this does not lead to changes in terms of convergence due to the optimality principle. Condition $t_f \in I$ in OCP has been replaced by $\Delta t_{\min} \le \Delta t_0 \le \Delta t_{\max}$ with bounds $\Delta t_{\min}, \Delta t_{\max} \in \mathbb{R}_+^0$. Furthermore, condition $\Delta t_k = \Delta t_{k+1}$ ensures uniformity between all time intervals. Notice that for the set evaluations $t_k = k\Delta t_0 = k\Delta t_k$ holds due to the uniform grid and choosing $k\Delta t_k$ preserves the local structure of the optimization problem.

## Receding-Horizon Quadratic-Form MPC

The most common MPC realization utilizes a quadratic form objective to minimize the quadratic control error and effort. But this metric must be adjusted to account for alternative state space representations. Common metrics for rotation groups are provided in [37], but here we include the nonlinear difference operator in the quadratic form:

$$ J_f(\mathbf{x}_N) = (\mathbf{x}_N \boxminus \mathbf{x}_f)^\top \mathbf{Q}_f (\mathbf{x}_N \boxminus \mathbf{x}_f), \\ \ell(\mathbf{x}_k, \mathbf{u}_k) = (\mathbf{x}_k \boxminus \mathbf{x}_f)^\top \mathbf{Q} (\mathbf{x}_k \boxminus \mathbf{x}_f) + \mathbf{u}_k^\top \mathbf{R} \mathbf{u}_k. $$

Hereby, $\mathbf{Q}, \mathbf{Q}_f \in \mathbb{R}^{p \times p}$ and $\mathbf{R} \in \mathbb{R}^{q \times q}$ denote weighting matrices with state dimension $p = \dim(\mathcal{R})$ and input dimension $q = \dim(\mathcal{U})$, respectively. Furthermore, $\mathbf{x}_f \in \mathbb{X}_f(t_n)$ represents the current intermediate goal state. In case the full goal state is not available, it is possible to either choose additional states to complete a steady state or to set related components in $\mathbf{Q}$ and $\mathbf{Q}_f$ to zero. Note that $\mathbf{x}_f$ could also be replaced by a time-dependent reference $\mathbf{x}_f(t)$ during optimization for tracking control. In this MPC realization, the horizon length resp. final time $t_f$ is fixed to $I = \{N\Delta t_s\}$ which implies $\Delta t_{\min} = \Delta t_{\max} = \Delta t_k = \Delta t_s$ with a predefined grid resolution $\Delta t_s \in \mathbb{R}^+$. Therefore the optimization w.r.t. time becomes obsolete and thus the optimization parameters $\Delta t_k$ with $k = 0, 1, \dots, N-1$ as well as temporal constraints are excluded to reduce the dimensions of the optimization problem.

Stability and recursive feasibility conditions for MPC usually rely on proper terminal conditions $J_f(\cdot)$ resp. $\mathbf{Q}_f$ and $\mathbb{X}_f(t_n)$ to, e.g., approximate an infinite horizon. Notice that a sampled data discrete-time representation follows by setting $\Delta t_s = 1$. For (output) path-following MPC, [9] provides terminal conditions and conditions on the reference path under which stability is guaranteed. However, as a suitable terminal region is difficult to determine, most practical approaches usually relay on sufficiently long horizon lengths to ensure stability and feasibility and choose $\mathbb{X}_f(t_n) := \mathcal{R}$. For static environments and a class of robotic systems, i.e. differential drive robots, [54] guarantees stability for shorter receding horizons.

## Obstacle Avoidance Constraints

Here, obstacles are defined as simply connected regions $\mathcal{O}_l(t), l = 1, \dots, L$ with $L$ as the number of obstacles. The time-dependency explicitly accounts for dynamic obstacles. Furthermore, let $\mathcal{R}(\mathbf{x})$ denote a geometric robot collision model dependent on the state $\mathbf{x} \in \mathcal{R}$, e.g. position and orientation. The minimum distance $d_l: \mathcal{R} \times \mathbb{R} \mapsto \mathbb{R}_+^0$ between the robot and obstacle $l$ is then determined by:

$$ d_l(\mathbf{x}, t) = \min_{\mathbf{p}_1 \in \mathcal{R}(\mathbf{x}), \mathbf{p}_2 \in \mathcal{O}_l(t)} \|\mathbf{p}_2 - \mathbf{p}_1\|_2. $$

By specifying a minimum separation to obstacles $d_{\min} \in \mathbb{R}_+^0$, the collision-free state space $\mathbb{X}_0(t)$ is given as follows:
$$ \mathcal{X}_o(t) = \{ \mathbf{x} \in \mathcal{X} \mid d_l(\mathbf{x}, t) \ge d_{\min}, l = 1, 2, \dots, L \}. $$

Furthermore, let the robot's internal state constraints are described by set $\mathcal{X}_i(t) \in \mathcal{X}$, then $\mathcal{X}(t)$ in OCP and NLP is given by $\mathcal{X}(t) = \mathcal{X}_0(t) \cap \mathcal{X}_i(t)$. Note that there exist techniques in the literature for computationally efficient optimization based obstacle avoidance.

## Solution of the Nonlinear Program

For NLP general necessary and sufficient optimality conditions apply [35]. Any practical implementation replaces constraint sets $\mathcal{X}, \mathcal{U}, \mathcal{U}_d$ and $\mathcal{X}_f$ by algebraic equality and inequality functions. The NLP is solved by standard local constrained optimization techniques, such as interior point methods or sequential quadratic programming. Warm-starting the NLPs in subsequent closed loop steps with the previous solution significantly increases performance. To comply with the alternative parameterizations, the solvers need to be modified in a few places. Let $\mathbf{z} = (\mathbf{u}_0, \mathbf{x}_0, \Delta t_0, \dots, \mathbf{u}_{N-1}, \mathbf{x}_{N-1}, \Delta t_{N-1}, \mathbf{x}_N)$ define the optimization parameter vector of NLP. Local solutions $\mathbf{z}^* = \mathbf{z} \oplus \Delta \mathbf{z}$ after each solver step follow by incrementing components as follows: $\mathbf{u}_k + \Delta \mathbf{u}_k, \Delta t_k + \Delta \Delta t_k$ and $\mathbf{x}_k \oplus \Delta \mathbf{x}_k$. Also the calculation of Jacobians and gradients w.r.t. $\mathbf{x}$ requires to apply proper increments. E.g. the Jacobian of some vector valued state-dependent function $\mathbf{g}(\cdot)$ evaluated at $\bar{\mathbf{x}}$ is given by:

$$ D_{\mathbf{x}} \mathbf{g}(\bar{\mathbf{x}}) = \frac{\partial \mathbf{g}(\bar{\mathbf{x}} \oplus \Delta \mathbf{x})}{\partial \Delta \mathbf{x}} \Big|_{\Delta \mathbf{x}=0} $$

Since second-order derivatives can be calculated numerically using quasi-Newton methods or finite differences from firstorder information, no dedicated increment is required.

The structure of NLP is sparse and utilizing sparse algebra for efficient optimization is crucial. While established NLP solvers like IPOPT [3] already support sparse algebra, it is possible to further speed up optimization by computing derivatives only for structured non-zeros. Similar to the TEB, NLP is represented as a hypergraph with cost functions and constraints as hyperedges and states, controls and time intervals as vertices. This facilitates the computation of sparse finite differences by iterating the edge set. A major advantage of the hypergraph is its simple algorithmic reconfiguration at negligible overhead. This is crucial for real-time control as the NLP dimensions change during runtime either due to dynamic environments or grid adaptation. Refer to [33] for general performance results and a detailed description on hypergraphs for MPC. More information about the method is detailed in [55].
$$ \mathcal{X}_o(t) = \{\mathbf{x} \in \mathcal{X} \mid d_l(\mathbf{x}, t) \ge d_{\min}, l = 1, 2, \dots, L\}. $$

Furthermore, let the robot's internal state constraints are described by set $\mathcal{X}_i(t) \in \mathcal{X}$, then $\mathcal{X}(t)$ in OCP and NLP is given by $\mathcal{X}(t) = \mathcal{X}_0(t) \cap \mathcal{X}_i(t)$. Note that there exist techniques in the literature for computationally efficient optimization based obstacle avoidance.

## Solution of the Nonlinear Program

For NLP general necessary and sufficient optimality conditions apply [35]. Any practical implementation replaces constraint sets $\mathcal{X}, \mathcal{U}, \mathcal{U}_d$ and $\mathcal{X}_f$ by algebraic equality and inequality functions. The NLP is solved by standard local constrained optimization techniques, such as interior point methods or sequential quadratic programming. Warm-starting the NLPs in subsequent closed loop steps with the previous solution significantly increases performance. To comply with the alternative parameterizations, the solvers need to be modified in a few places. Let $\mathbf{z} = (\mathbf{u}_0, \mathbf{x}_0, \Delta t_0, \dots, \mathbf{u}_{N-1}, \mathbf{x}_{N-1}, \Delta t_{N-1}, \mathbf{x}_N)$ define the optimization parameter vector of NLP. Local solutions $\mathbf{z}^* = \mathbf{z} \oplus \Delta \mathbf{z}$ after each solver step follow by incrementing components as follows: $\mathbf{u}_k + \Delta \mathbf{u}_k, \Delta t_k + \Delta \Delta t_k$ and $\mathbf{x}_k \oplus \Delta \mathbf{x}_k$. Also the calculation of Jacobians and gradients w.r.t. $\mathbf{x}$ requires to apply proper increments. E.g. the Jacobian of some vector valued state-dependent function $\mathbf{g}(\cdot)$ evaluated at $\bar{\mathbf{x}}$ is given by:

$$ D_{\mathbf{x}}\mathbf{g}(\bar{\mathbf{x}}) = \frac{\partial \mathbf{g}(\bar{\mathbf{x}} \oplus \Delta \mathbf{x})}{\partial \Delta \mathbf{x}} \Big|_{\Delta \mathbf{x}=0} $$

Since second-order derivatives can be calculated numerically using quasi-Newton methods or finite differences from firstorder information, no dedicated increment is required.

The structure of NLP is sparse and utilizing sparse algebra for efficient optimization is crucial. While established NLP solvers like IPOPT [3] already support sparse algebra, it is possible to further speed up optimization by computing derivatives only for structured non-zeros. Similar to the TEB, NLP is represented as a hypergraph with cost functions and constraints as hyperedges and states, controls and time intervals as vertices. This facilitates the computation of sparse finite differences by iterating the edge set. A major advantage of the hypergraph is its simple algorithmic reconfiguration at negligible overhead. This is crucial for real-time control as the NLP dimensions change during runtime either due to dynamic environments or grid adaptation. Refer to [33] for general performance results and a detailed description on hypergraphs for MPC. More information about the method is detailed in [55].
Bibliography

[1] VICTOR ANDOH. Autonomous mobile robots, 2021. https://www.allaboutlean.com/amazon-robotics-family/.

[2] Mihai-Constantin Balaşa, Stefan Cuculici, Cosmin Pantu, Simona Mihai, Negrea Alexis, Mihai-Octavian Zdrafcu, Dorin Let, Viviana Filip, and S. Cristea. Using 3d scanning techniques in orthopedic systems modeling. *Scientific Bulletin of Valahia University - Materials and Mechanics*, 15, 10 2017.

[3] Subhrajit Bhattacharya and Maxim Likhachev. Identification and representation of homotopy classes of trajectories for search-based path planning in 3d. In *Identification and Representation of Homotopy Classes of Trajectories for Search-based Path Planning in 3D*, 06 2011.

[4] Alexander Botros, Nils Wilde, and Stephen L. Smith. Learning control sets for lattice planners from user preferences. In *Steven M. LaValle, Ming Lin, Timo Ojala, Dylan Shell, and Jingjin Yu, editors, Algorithmic Foundations of Robotics XIV*, pages 381–397, Cham, 2021. Springer International Publishing.

[5] JohnTravis Butler and Arvin Agah. Psychological effects of behavior patterns of a mobile personal robot. *Autonomous Robots*, 10:185–202, 03 2001.

[6] Manue Fernandez Carmona, Laurence Roberts Elliott, Marc Hanheide, and Ravi Teja Chadalavada. Intra-logistics with integrated automatic deployment safe and scalable fleets in shared spaces, 2020. http://iliad-project.eu/wp-content/uploads/Deliverables/D3_3.pdf.

[7] Howie Choset, Kevin Lynch, Seth Hutchinson, George Kantor, Wolfram Burgard, Lydia Kavraki, and Se-bastian Thrun. *Principles of Robot Motion: Theory, Algorithms, and Implementations*, chapter 1-25. choset, 01 2005.

[8] conger. Amrs vs. agvs: The differences explained, 2023. https://www.conger.com/amr-vs-agv/.

[9] Jérémie Deray, Bence Magyar, Joan Solà, and Juan Andrade-Cetto. Timed-elastic smooth curve optimization for mobile-base motion planning. In *2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, pages 3143–3149, 2019.

[10] fedralbrace. Point load vs. uniform distributed load, 2023. https://www.federalbrace.com/content/306-point-load-vs-uniform-distributed-load.

[11] Manuel Fernández-Carmona, Tejas P. Parekh, and Marc Hanheide. Making the case for human-aware navigation in warehouses. In *Towards Autonomous Robotic Systems*, 2019.

[12] International Organization for Standardization. Ergonomics — Manual handling —. Standard ISO 11228:2007(E), International Organization for Standardization, Geneva, CH, April 2007.

[13] D. Fox, W. Burgard, and S. Thrun. The dynamic window approach to collision avoidance. *IEEE Robotics and Automation Magazine*, 4(1):23–33, 1997.

[14] Giuseppe Fragapane, Mirco Peron and Fabio Sgarbossa, and Jan Ola Strandhagen. Increasing flexibility and productivity in industry 4.0 production networks with autonomous mobile robots and smart intralogistics. *Artificial Intelligence in Operations Management*, page 125–143, 2020.

[15] Gibas. mir100 user guide, 2020. https://gibas.nl/wp-content/uploads/2021/01/mir100-user-guide_31_en.pdf.
Bibliography

[1] VICTOR ANDOH. Autonomous mobile robots, 2021. https://www.allaboutlean.com/amazon-robotics-family/.

[2] Mihai-Constantin Balaşa, Stefan Cuculici, Cosmin Pantu, Simona Mihai, Negrea Alexis, Mihai-Octavian Zdrafcu, Dorin Let, Viviana Filip, and S. Cristea. Using 3d scanning techniques in orthopedic systems modeling. *Scientific Bulletin of Valahia University - Materials and Mechanics*, 15, 10 2017.

[3] Subhrajit Bhattacharya and Maxim Likhachev. Identification and representation of homotopy classes of trajectories for search-based path planning in 3d. In *Identification and Representation of Homotopy Classes of Trajectories for Search-based Path Planning in 3D*, 06 2011.

[4] Alexander Botros, Nils Wilde, and Stephen L. Smith. Learning control sets for lattice planners from user preferences. In *Steven M. LaValle, Ming Lin, Timo Ojala, Dylan Shell, and Jingjin Yu, editors, Algorithmic Foundations of Robotics XIV*, pages 381–397, Cham, 2021. Springer International Publishing.

[5] JohnTravis Butler and Arvin Agah. Psychological effects of behavior patterns of a mobile personal robot. *Autonomous Robots*, 10:185–202, 03 2001.

[6] Manue Fernandez Carmona, Laurence Roberts Elliott, Marc Hanheide, and Ravi Teja Chadalavada. Intra-logistics with integrated automatic deployment safe and scalable fleets in shared spaces, 2020. http://iliad-project.eu/wp-content/uploads/Deliverables/D3_3.pdf.

[7] Howie Choset, Kevin Lynch, Seth Hutchinson, George Kantor, Wolfram Burgard, Lydia Kavraki, and Se-bastian Thrun. *Principles of Robot Motion: Theory, Algorithms, and Implementations*, chapter 1-25. choset, 01 2005.

[8] conger. Amrs vs. agvs: The differences explained, 2023. https://www.conger.com/amr-vs-agv/.

[9] Jérémie Deray, Bence Magyar, Joan Solà, and Juan Andrade-Cetto. Timed-elastic smooth curve optimization for mobile-base motion planning. In *2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, pages 3143–3149, 2019.

[10] fedralbrace. Point load vs. uniform distributed load, 2023. https://www.federalbrace.com/content/306-point-load-vs-uniform-distributed-load.

[11] Manuel Fernández-Carmona, Tejas P. Parekh, and Marc Hanheide. Making the case for human-aware navigation in warehouses. In *Towards Autonomous Robotic Systems*, 2019.

[12] International Organization for Standardization. Ergonomics — Manual handling —. Standard ISO 11228:2007(E), International Organization for Standardization, Geneva, CH, April 2007.

[13] D. Fox, W. Burgard, and S. Thrun. The dynamic window approach to collision avoidance. *IEEE Robotics and Automation Magazine*, 4(1):23–33, 1997.

[14] Giuseppe Fragapane, Mirco Peron and Fabio Sgarbossa, and Jan Ola Strandhagen. Increasing flexibility and productivity in industry 4.0 production networks with autonomous mobile robots and smart intralogistics. *Artificial Intelligence in Operations Management*, page 125–143, 2020.

[15] Gibas. mir100 user guide, 2020. https://gibas.nl/wp-content/uploads/2021/01/mir100-user-guide_31_en.pdf.
[16] Alan Godfrey, Richard Conway, David Meagher, and Gearóid ÓLaighin. Direct measurement of human movement by accelerometry. *Medical engineering and physics*, 30:1364–86, 01 2009.

[17] Ruchi Goel and Pooja Gupta. Robotics and Industry 4.0, pages 157–169. Springer International Publishing, Cham, 2020.

[18] Rajkumar Gunasekar and Naveen Kumar Chandramohan. Development of image acquisition system to eleminate blind spot of a-pillar. *International Journal of Scientific Research Management Studies*, 8:145–151, 10 2018.

[19] Rajkumar Gunasekar and Naveen Kumar Chandramohan. Development of image acquisition system to eleminate blind spot of a-pillar. *International Journal of Scientific Research and Management Studies*, 8:145–151, 10 2018.

[20] E.T. Hall, E.T. Hall, and Copyright Paperback Collection (Library of Congress). *The Hidden Dimension*. Anchor books. Knopf Doubleday Publishing Group, 1966.

[21] HarbH. Manual handling demo, 2023. https://youtube.com/ playlist?list=PLgEV96JTa94DeYLKeABMZ7SYYi2PHM-wr.

[22] Thomas Howard, Colin Green, Alonzo Kelly, and Dave Ferguson. State space sampling of feasible motions for high performance mobile robot navigation in complex environments. *J. Field Robotics*, 25:325–345, 06 2008.

[23] Thomas Howard, Mihail Pivtoraiko, Ross A. Knepper, and Alonzo Kelly. Model-predictive motion planning: Several key developments for autonomous mobile robots. *IEEE Robotics and Automation Magazine*, 21(1):64–73, 2014.

[24] Yamin Huang, Linying Chen, Pengfei Chen, R.R. Negenborn, and P.H.A.J.M. Gelder. Ship collision avoidance methods: State-of-the-art. In *Safety Science*, pages 1182–1187, 01 2020.

[25] ifr. Autonomous mobile robot for transport and delivery of materia, 2020. https://ifr.org/ ifr-press-releases/news/AGV-for-transport-and-delivery-of-material.

[26] Léonard Jaillet, Juan Cortés, and Thierry Siméon. Sampling-based path planning on configuration-space costmaps. *IEEE Transactions on Robotics*, 26(4):635–646, 2010.

[27] Lucas Janson, Edward Schmerling, Ashley Clark, and Marco Pavone. Fast marching tree: A fast marching sampling-based method for optimal motion planning in many dimensions. *The International Journal of Robotics Research*, 34(7):883–921, 2015. PMID: 27003958.

[28] Adam Kendon. *Spacing and Orientation in Co-present Interaction*, pages 1–15. Springer Berlin Heidelberg, Berlin, Heidelberg, 2010.

[29] R. Kirby. *Social robot navigation*. PhD dissertation, Carnegie Mellon University, 2010.

[30] Kay Kitazawa and Taku Fujiyama. Pedestrian vision and collision avoidance behavior: Investigation of the information process space of pedestrians using an eye tracker. In Wolfram W. F. Klingsch, Christian Rogsch, Andreas Schadschneider, and Michael Schreckenberg, editors, *Pedestrian and Evacuation Dynamics 2008*, pages 95–108, Berlin, Heidelberg, 2010. Springer Berlin Heidelberg.

[31] Sarah Knapp. Midsagittal plane, 2020. https://biologydictionary.net/midsagittal-plane/.

[32] koerber supplychain. Autonomous mobile robots, 2023. https://www.koerber-supplychain.com/ supply-chain-solutions/warehouse-robots/autonomous-mobile-robots/.

[33] Jason Kong, Mark Pfeiffer, Georg Schildbach, and Francesco Borrelli. Kinematic and dynamic vehicle models for autonomous driving control design. In *2015 IEEE Intelligent Vehicles Symposium (IV)*, pages 1094–1099, 2015.

[34] Thibault Kruse, Amit Kumar Pandey, Rachid Alami, and Alexandra Kirsch. Human-aware robot navigation: A survey. *Robotics and Autonomous Systems*, 61(12):1726–1743, 2013.
[16] Alan Godfrey, Richard Conway, David Meagher, and Gearóid ÓLaighin. Direct measurement of human movement by accelerometry. *Medical engineering and physics*, 30:1364–86, 01 2009.

[17] Ruchi Goel and Pooja Gupta. Robotics and Industry 4.0, pages 157–169. Springer International Publishing, Cham, 2020.

[18] Rajkumar Gunasekar and Naveen Kumar Chandramohan. Development of image acquisition system to eleminate blind spot of a-pillar. *International Journal of Scientific Research Management Studies*, 8:145–151, 10 2018.

[19] Rajkumar Gunasekar and Naveen Kumar Chandramohan. Development of image acquisition system to eleminate blind spot of a-pillar. *International Journal of Scientific Research and Management Studies*, 8:145–151, 10 2018.

[20] E.T. Hall, E.T. Hall, and Copyright Paperback Collection (Library of Congress). *The Hidden Dimension*. Anchor books. Knopf Doubleday Publishing Group, 1966.

[21] HarbH. Manual handling demo, 2023. https://youtube.com/ playlist?list=PLgEV96JTa94DeYLKeABMZ7SYYi2PHM-wr.

[22] Thomas Howard, Colin Green, Alonzo Kelly, and Dave Ferguson. State space sampling of feasible motions for high performance mobile robot navigation in complex environments. *J. Field Robotics*, 25:325–345, 06 2008.

[23] Thomas Howard, Mihail Pivtoraiko, Ross A. Knepper, and Alonzo Kelly. Model-predictive motion planning: Several key developments for autonomous mobile robots. *IEEE Robotics and Automation Magazine*, 21(1):64–73, 2014.

[24] Yamin Huang, Linying Chen, Pengfei Chen, R.R. Negenborn, and P.H.A.J.M. Gelder. Ship collision avoidance methods: State-of-the-art. In *Safety Science*, pages 1182–1187, 01 2020.

[25] ifr. Autonomous mobile robot for transport and delivery of materia, 2020. https://ifr.org/ ifr-press-releases/news/AGV-for-transport-and-delivery-of-material.

[26] Léonard Jaillet, Juan Cortés, and Thierry Siméon. Sampling-based path planning on configuration-space costmaps. *IEEE Transactions on Robotics*, 26(4):635–646, 2010.

[27] Lucas Janson, Edward Schmerling, Ashley Clark, and Marco Pavone. Fast marching tree: A fast marching sampling-based method for optimal motion planning in many dimensions. *The International Journal of Robotics Research*, 34(7):883–921, 2015. PMID: 27003958.

[28] Adam Kendon. *Spacing and Orientation in Co-present Interaction*, pages 1–15. Springer Berlin Heidelberg, Berlin, Heidelberg, 2010.

[29] R. Kirby. *Social robot navigation*. PhD dissertation, Carnegie Mellon University, 2010.

[30] Kay Kitazawa and Taku Fujiyama. Pedestrian vision and collision avoidance behavior: Investigation of the information process space of pedestrians using an eye tracker. In Wolfram W. F. Klingsch, Christian Rogsch, Andreas Schadschneider, and Michael Schreckenberg, editors, *Pedestrian and Evacuation Dynamics 2008*, pages 95–108, Berlin, Heidelberg, 2010. Springer Berlin Heidelberg.

[31] Sarah Knapp. Midsagittal plane, 2020. https://biologydictionary.net/midsagittal-plane/.

[32] koerber supplychain. Autonomous mobile robots, 2023. https://www.koerber-supplychain.com/ supply-chain-solutions/warehouse-robots/autonomous-mobile-robots/.

[33] Jason Kong, Mark Pfeiffer, Georg Schildbach, and Francesco Borrelli. Kinematic and dynamic vehicle models for autonomous driving control design. In *2015 IEEE Intelligent Vehicles Symposium (IV)*, pages 1094–1099, 2015.

[34] Thibault Kruse, Amit Kumar Pandey, Rachid Alami, and Alexandra Kirsch. Human-aware robot navigation: A survey. *Robotics and Autonomous Systems*, 61(12):1726–1743, 2013.
[35] Rainer Kümmerle, Giorgio Grisetti, Hauke Strasdat, Kurt Konolige, and Wolfram Burgard. G2o: A general framework for graph optimization. In *2011 IEEE International Conference on Robotics and Automation*, pages 3607–3613, 2011.

[36] Volker Lauff and Frank Werner. Scheduling with common due date, earliness and tardiness penalties for multimachine problems: A survey. *Mathematical and Computer Modelling*, 40:637–655, 09 2004.

[37] Steven LaValle. *Planning Algorithms*. Cambridge University Press, 01 2006.

[38] Felix Lindner and Carola Eschenbach. Towards a formalization of social spaces for socially aware robots. In Max Egenhofer, Nicholas Giudice, Reinhard Moratz, and Michael Worboys, editors, *Spatial Information Theory*, pages 283–303, Berlin, Heidelberg, 2011. Springer Berlin Heidelberg.

[39] Yuanchang Liu, Richard Bucknall, and Xinyu Zhang. The fast marching method based intelligent navigation of an unmanned surface vehicle. *Ocean Engineering*, 142:363–376, 09 2017.

[40] Jim Mainprice, E. Akin Sisbot, Léonard Jaillet, Juan Cortés, Rachid Alami, and Thierry Siméon. Planning human-aware motions using a sampling-based costmap planner. In *2011 IEEE International Conference on Robotics and Automation*, pages 5012–5017, 2011.

[41] Jim Mainprice, Mamoun Gharbi, Thierry Siméon, and Rachid Alami. Sharing effort in planning human-robot handover tasks. In *2012 IEEE RO-MAN: The 21st IEEE International Symposium on Robot and Human Interactive Communication*, pages 764–770, 2012.

[42] Matlab. Anatomical position, 2014. https://www.mathworks.com/help/mpc/gs/what-is-mpc.html.

[43] Ashley Mauldin. Anatomical position, 2014. https://www.osmosis.org/answers/anatomical-position.

[44] MiR. Agv vs amr - what's the difference?, 2020. https://www.mobile-industrial-robots.com/insights/get-started-with-amrs/agv-vs-amr-whats-the-difference/.

[45] Thulasi Mylvaganam and Mario Sassano. Autonomous collision avoidance for wheeled mobile robots using a differential game approach. *European Journal of Control*, 40:53–61, 2018.

[46] Isira Naotunna and Theeraphong Wongratanaphisan. Comparison of ros local planners with differential drive heavy robotic system. In *2020 International Conference on Advanced Mechatronic Systems (ICAMechS)*, pages 1–6, 2020.

[47] Anish Pandey, Saroj Kumar, Krishna Kant Pandey, and Dayal R. Parhi. Mobile robot navigation in unknown static environments using anfis controller. *Perspectives in Science*, 8:421–423, 2016. Recent Trends in Engineering and Material Sciences.

[48] Mihail Pivtoraiko and Alonzo Kelly. Constrained motion planning in discrete state spaces. In *International Symposium on Field and Service Robotics*, 2005.

[49] Morgan Quigley, Ken Conley, Brian Gerkey, Josh Faust, Tully Foote, Jeremy Leibs, Rob Wheeler, and Andrew Ng. Ros: an open-source robot operating system. In *null*, volume 3, 01 2009.

[50] Marina Raineri, Simone Perri, and Corrado Guarino Lo Bianco. Safety and efficiency management in lgv operated warehouses. *Robotics and Computer-Integrated Manufacturing*, 57:73–85, 2019.

[51] Rafael Rey, Jose Antonio Cobano, Marco Corzetto, Luis Merino, Paulo Alvito, and Fernando Caballero. A novel robot co-worker system for paint factories without the need of existing robotic infrastructure. *Robotics and Computer-Integrated Manufacturing*, 70:102122, 2021.

[52] Richards and Gwynne. *Warehouse Management: A complete guide to improving efficiency and minimising costs in the modern warehouse*. Kogan, 06 2011.

[53] Jorge Rios-Martinez, Anne Spalanzani, and Christian Laugier. From proxemics theory to socially-aware navigation: A survey. *International Journal of Social Robotics*, 7:137–153, 04 2014.
[35] Rainer Kümmerle, Giorgio Grisetti, Hauke Strasdat, Kurt Konolige, and Wolfram Burgard. G2o: A general framework for graph optimization. In *2011 IEEE International Conference on Robotics and Automation*, pages 3607–3613, 2011.

[36] Volker Lauff and Frank Werner. Scheduling with common due date, earliness and tardiness penalties for multimachine problems: A survey. *Mathematical and Computer Modelling*, 40:637–655, 09 2004.

[37] Steven LaValle. *Planning Algorithms*. Cambridge University Press, 01 2006.

[38] Felix Lindner and Carola Eschenbach. Towards a formalization of social spaces for socially aware robots. In Max Egenhofer, Nicholas Giudice, Reinhard Moratz, and Michael Worboys, editors, *Spatial Information Theory*, pages 283–303, Berlin, Heidelberg, 2011. Springer Berlin Heidelberg.

[39] Yuanchang Liu, Richard Bucknall, and Xinyu Zhang. The fast marching method based intelligent navigation of an unmanned surface vehicle. *Ocean Engineering*, 142:363–376, 09 2017.

[40] Jim Mainprice, E. Akin Sisbot, Léonard Jaillet, Juan Cortés, Rachid Alami, and Thierry Siméon. Planning human-aware motions using a sampling-based costmap planner. In *2011 IEEE International Conference on Robotics and Automation*, pages 5012–5017, 2011.

[41] Jim Mainprice, Mamoun Gharbi, Thierry Siméon, and Rachid Alami. Sharing effort in planning human-robot handover tasks. In *2012 IEEE RO-MAN: The 21st IEEE International Symposium on Robot and Human Interactive Communication*, pages 764–770, 2012.

[42] Matlab. Anatomical position, 2014. https://www.mathworks.com/help/mpc/gs/what-is-mpc.html.

[43] Ashley Mauldin. Anatomical position, 2014. https://www.osmosis.org/answers/anatomical-position.

[44] MiR. Agv vs amr - what's the difference?, 2020. https://www.mobile-industrial-robots.com/insights/get-started-with-amrs/agv-vs-amr-whats-the-difference/.

[45] Thulasi Mylvaganam and Mario Sassano. Autonomous collision avoidance for wheeled mobile robots using a differential game approach. *European Journal of Control*, 40:53–61, 2018.

[46] Isira Naotunna and Theeraphong Wongratanaphisan. Comparison of ros local planners with differential drive heavy robotic system. In *2020 International Conference on Advanced Mechatronic Systems (ICAMechS)*, pages 1–6, 2020.

[47] Anish Pandey, Saroj Kumar, Krishna Kant Pandey, and Dayal R. Parhi. Mobile robot navigation in unknown static environments using anfis controller. *Perspectives in Science*, 8:421–423, 2016. Recent Trends in Engineering and Material Sciences.

[48] Mihail Pivtoraiko and Alonzo Kelly. Constrained motion planning in discrete state spaces. In *International Symposium on Field and Service Robotics*, 2005.

[49] Morgan Quigley, Ken Conley, Brian Gerkey, Josh Faust, Tully Foote, Jeremy Leibs, Rob Wheeler, and Andrew Ng. Ros: an open-source robot operating system. In *null*, volume 3, 01 2009.

[50] Marina Raineri, Simone Perri, and Corrado Guarino Lo Bianco. Safety and efficiency management in lgv operated warehouses. *Robotics and Computer-Integrated Manufacturing*, 57:73–85, 2019.

[51] Rafael Rey, Jose Antonio Cobano, Marco Corzetto, Luis Merino, Paulo Alvito, and Fernando Caballero. A novel robot co-worker system for paint factories without the need of existing robotic infrastructure. *Robotics and Computer-Integrated Manufacturing*, 70:102122, 2021.

[52] Richards and Gwynne. *Warehouse Management: A complete guide to improving efficiency and minimising costs in the modern warehouse*. Kogan, 06 2011.

[53] Jorge Rios-Martinez, Anne Spalanzani, and Christian Laugier. From proxemics theory to socially-aware navigation: A survey. *International Journal of Social Robotics*, 7:137–153, 04 2014.