Yusheng Xiang

**Band 97**

Y. Xiang

AI and IoT Meet Mobile Machines: Towards a Smart Working Site

**AI and IoT Meet Mobile Machines: Towards a Smart Working Site**

Yusheng Xiang

**AI and IoT Meet Mobile Machines: Towards a Smart Working Site**

#### **Karlsruher Schriftenreihe Fahrzeugsystemtechnik Band 97**

Herausgeber

#### **FAST Institut für Fahrzeugsystemtechnik** Prof. Dr. rer. nat. Frank Gauterin Prof. Dr.-Ing. Marcus Geimer Prof. Dr.-Ing. Peter Gratzfeld Prof. Dr.-Ing. Frank Henning

Das Institut für Fahrzeugsystemtechnik besteht aus den Institutsteilen Bahnsystemtechnik, Fahrzeugtechnik, Leichtbautechnologie und Mobile Arbeitsmaschinen.

Eine Übersicht aller bisher in dieser Schriftenreihe erschienenen Bände finden Sie am Ende des Buchs.

# **AI and IoT Meet Mobile Machines: Towards a Smart Working Site**

by Yusheng Xiang

Karlsruher Institut für Technologie Institut für Fahrzeugsystemtechnik

AI and IoT Meet Mobile Machines: Towards a Smart Working Site

Zur Erlangung des akademischen Grades eines Doktors der Ingenieurwissenschaften von der KIT-Fakultät für Maschinenbau des Karlsruher Instituts für Technologie (KIT) genehmigte Dissertation

von Yusheng Xiang, M.Sc.

Tag der mündlichen Prüfung: 22. September 2021 Hauptreferent: Prof. Dr.-Ing. Marcus Geimer Korreferent: Prof. Dr.-Ing. Tamim Asfour

#### **Impressum**

Karlsruher Institut für Technologie (KIT) KIT Scientific Publishing Straße am Forum 2 D-76131 Karlsruhe

KIT Scientific Publishing is a registered trademark of Karlsruhe Institute of Technology. Reprint using the book cover is not allowed.

www.ksp.kit.edu

*This document – excluding parts marked otherwise, the cover, pictures and graphs – is licensed under a Creative Commons Attribution 4.0 International License (CC BY 4.0): https://creativecommons.org/licenses/by/4.0/deed.en*

*The cover page is licensed under a Creative Commons Attribution-No Derivatives 4.0 International License (CC BY-ND 4.0): https://creativecommons.org/licenses/by-nd/4.0/deed.en*

Print on Demand 2022 – Gedruckt auf FSC-zertifiziertem Papier

ISSN 1869-6058 ISBN 978-3-7315-1165-6 DOI 10.5445/KSP/1000143073

# **Vorwort des Herausgebers**

Methoden der künstlichen Intelligenz (KI) rücken zunehmend in den öffentlichen Fokus.  Maschinelle Lernverfahren (ML) und Künstlich Neuronale Netze (KNN) sind heute sehr häufig erforschte Methoden der KI.

 Es gibt kaum einen Ort auf der Welt, an dem man nicht ins Internet der Dinge (IoT) gelangen kann.

Die Karlsruher Schriftenreihe Fahrzeugsystemtechnik widmet sich Themen der Steuerung und der Digitalisierung von Fahrzeugen.

Großes Potential bieten KI- und IoT-Methoden aber auch im Bereich der Logistik auf Großbaustellen. Die Planung von Fahrwegen der Fahrzeuge und der einzelnen Arbeitsaktivitäten sind Beispiele für eine Optimierung, die in diesem Band 97 gezeigt werden. Herr Xiang wählt die Großbaustelle in Wuhan zum Bau des Huoshenshan-Krankenhauses als Beispiel zur Motivation seiner Arbeit.  Im Weiteren zeigt er, wie er mit einem Mehr-Ebenen-Layer eine Karte in Echtzeit erstellen kann, mit dessen Hilfe Arbeitsmaschinen eine Pfadplanung durchführen können.   Auch entwickelt er ein umfangreiches Datenset zur Erkennung von mobilen Arbeitsmaschinen. Er nutzt dazu den YOLOv3 Algorithmus und erreicht Erkennungsraten von deutlich über 80%. Nicht zuletzt vergleicht er eine Kommunikation zwischen Maschinen auf Basis von IEEE 802.11 (WLAN) und 5G. Auch wenn das 5G-Netz nachvollziehbar wesentlich höhere Datenübertragungsraten in den von Herrn Xiang untersuchten Szenarien erreicht, so zeigt er auch die Nachteile bei Fahrzeuggeschwindigkeiten oberhalb von 40 km/h.

Karlsruhe, im September 2021 Prof. Dr.-Ing. Marcus Geimer

# **Preface**

The dissertation includes some parts of my research during the last three years as a PhD student, research scientist, and visiting scholar at Karlsruhe Institute of Technology, Robert Bosch LLC, and University of California, Berkeley. Here I would like to sincerely thank some people who did contribute to my dissertation and my growth.

First of all, I am very grateful for the guidance from Prof. Dr. Marcus Geimer at KIT. During the time when we taught lectures at the university, Prof. Geimer shows me what means responsible and rigorous with action. Also, I appreciate the time we generated novel ideas and wrote academic papers together. It is my pleasure to cooperate with such a great educator. His virtues will affect me for my whole life.

Also, I would like to thank Dr. Steffen Mutschler at Bosch Rexroth, who trusts and offers me this opportunity to do my research at a prestigious university and company. During the early phase of my research, he guided me thoroughly and patiently to become familiar with the projects quickly. Without a doubt, he did an outstanding contribution to my personal and professional growth in my PhD early stage. All my "achievements" during my PhD time cannot be accomplished without him.

Another important person is Dr. Christine Brach at Bosch Rexroth. After Dr. Steffen Mutschler left our department, she took the responsibility to guide me and spent plenty of time on me, and even she must be extremely busy as a department leader. Her support much accelerates my projects and research. Also, her tolerance for my unconventional viewpoints guarantees the innovation of my research. Her openness, wisdom, and encouragement broadened my horizon and let me have the freedom to consider problems at a higher level.

Prof. Dr. Samuel S.Mao at UC, Berkeley makes me realize the difference between tiny innovation and disruptive innovation. Inspired by the conversations with him, I have a more in-depth insight into Silicon Valley's charm and comprehensively understand the idea of making human life better. As an outstanding professor with more than 46,000 citations and a pioneer to transfer the research results into the industry through high-tech startups, he guides me to continue to break through my limits. An outstanding research in engineering convinces both professors and venture capital partners.

I sincerely thank Dipl.-Ing. Norman Brix for his valuable expertise and impressive know-how in both software and hardware. What's more, he is my best German language teacher.

In addition, my special thank gives to the company, Robert Bosch LLC, where I gained knowledge and happiness at the same time in the past four years, started from internship. Besides these thanks, I am grateful for my graduate- and undergraduate students who supported me with their great enthusiasm and aptitude, and Karlsruhe House of Young Scientist for providing me the scholarship to visit abroad.

Finally, I would like to thank my partner, M.Sc. Tianqing Su. Her excellent programming skill and in-depth knowledge in the field of computer science help me overcome many technical difficulties. Not to mention, she always encouraged and accompanied me during the time before deadlines and appointments, making these hard times colorful and beautiful. Moreover, I appreciate her patience to listen the content of my "boring" dissertation almost every day.

Thank you!

Yusheng Xiang on Mar. 01, 2021

# **Abstract**

Infrastructure construction is society's cornerstone and economics' catalyst. Therefore, improving mobile machinery's efficiency and reducing their cost of use have enormous economic benefits in the vast and growing construction market. For this purpose, many methods have been proposed by industry and academia during the past few decades which contribute to even better products. As research in this area becomes more mature, significant optimization of single construction machine is less likely to exist. Therefore, instead of focusing on improving the performance of single construction machinery, I considered a group of construction machinery as a whole system to improve the productivity of the working site. In this thesis, I envision a novel concept smart working site to increase productivity through fleet management from multiple aspects and with Artificial Intelligence (AI) and Internet of Things (IoT).

Investigating the famous construction site for the hospital, namely Huoshenshan, where the project was finished at an unprecedented speed in Wuhan during the coronavirus outbreak in 2020, the most impressive distinguishing features can be concluded as a large amount of machines investment and the well-ordered coordination. Inspired by this particular working site, this thesis aims to present the approaches to substitute some human coordinators using AI and IoT and thus make the concept of a smart working site offering high productivity closer to reality.

Firstly, I introduced a novel multi working-machines pathfinding algorithm to solve the path conflicts among machines and prioritize the more critical machines. The proposed algorithm outperforms the State-of-the-Art (SOTA) solution in pathfinding time, whereas it achieves optimal solution. To navigate the optimal path from the start point to the destination, an accurate localization, and mapping algorithm is indispensable. Therefore, a multi GPS/IMUs Simultaneous Localization And Mapping (SLAM) system based on commodity sensors on account of dynamically changing of the working site and noise of sensors was developed. This SLAM system offers the location and terrain information to supporting the successful path planning. Since some difficult tasks may still be finished by human drivers in the next decade, I endow my AI system the capability to predict the motion of manned machines. Concretely, I introduced combined neural networks to detect the manned machines' working process and validated with experimental data did on a wheel loader. Because the selected combined neural network is more suitable for transfer learning compared to the SOTA solution of the Multivariable Time Series Classification algorithm, my deep learning model has better generalization capability on different working sites and is more robust against the diversity of construction machines. Then, I created a visual monitoring system for the safety of participants without localization equipment. Given that the machines in a closed site can be treated as an L4 automation driving task, I built a mobile machines dataset to be used as a base dataset to train the SOTA deep-learning-based visual algorithm. By taking full advantage of L4 features, I proved my approach is extremely effective. To share all the information mentioned above between the command center and construction machines, I evaluated two major wireless communication systems for working sites, i.e., WLAN-based IEEE 802.11p and cellular network 5G, to achieve the seamless share of the large volume of information. The research about 5G indicates the working site setup, and the research about ad-hoc networks presents the handover strategy.

This thesis contributes to the path making Wuhan's speed a normal speed in the future, by quantitatively evaluating feasibility considering cutting-edge AI and IoT technologies.

Keywords: Smart Working Site, Multi Working Machine Pathfinding Algorithm, Multivariable Time Series Classification Algorithm, Mobile Machines Dataset, SLAM, 5G, IEEE 802.11p

# **Kurzfassung**

Der Bau von Infrastrukturen ist ein Eckpfeiler der Gesellschaft und ein Katalysator der Wirtschaft.  Zu diesem Zweck wurden in den letzten Jahrzehnten viele Methoden von Industrie und Wissenschaft vorgeschlagen, die zu noch besseren und leistungsfähigeren Produkten beitragen. Da die Forschung in diesem Bereich reifer wird, ist es weniger wahrscheinlich, dass eine signifikante Optimierung einer einzelnen Baumaschine vorliegt.

 Der vorgeschlagene Algorithmus übertrifft die SOTA-Lösung (State-of-the-Art) in der Rechenzeit, während eine optimale Lösung erzielt wird. Um den optimalen Weg vom Startpunkt zum Ziel zu navigieren, ist ein genauer Lokalisierungs- und Zuordnungsalgorithmus unerlässlich. Aus diesem Grund habe ich ein SLAM-System (Simultaneous Localization And Mapping), das auf Warensensoren basiert, da sich die Arbeitsstelle dynamisch ändert und das Rauschen der Sensoren auftritt. Dieses SLAM-System bietet Lokalisierungs- und Geländeinformationen zur Unterstützung der erfolgreichen Routenplanung. Da einige schwierige Aufgaben in den nächsten zehn Jahren möglicherweise noch von Menschen erledigt werden, kann ich auch auf der intelligenten Baustelle nicht auf diese verzichten. Konkret habe ich kombinierte neuronale Netze eingeführt, um den Arbeitsprozess der bemannten Maschinen zu erfassen und mit experimentellen Daten validiert, die mit einem Radlader erstellt wurden. Da das ausgewählte kombinierte neuronale Netzwerk im Vergleich mit SOTA-Lösung zur Klassifizierung multivariabler Zeitreihen besser für das Transferlernen geeignet ist, verfügt unser Deep-Learning-Modell über eine bessere Generalisierungsfähigkeit auf verschiedenen Arbeitsplätzen und ist robuster gegenüber der Vielfalt von Baumaschinen. Anschließend habe ich ein visuelles Überwachungssystem für die Sicherheit der Teilnehmer ohne Lokalisierungsausrüstung erstellt. Da die Maschinen an einem geschlossenen Standort als L4 des automatisiertes Fahren behandelt werden können, habe ich einen Datensatz für mobile Maschinen erstellt, der als Basisdatensatz zum Trainieren des auf SOTA Deep Learning basierenden visuellen Algorithmus verwendet werden kann.  Um alle oben genannten Informationen zwischen der Kommandozentrale und den Baumaschinen auszutauschen, habe ich zwei wichtige drahtlose Kommunikationssysteme für Arbeitsstätten evaluiert, nämlich WLAN-basiertes IEEE 802.11p und Mobilfunknetz 5G, um den nahtlosen Austausch des großen Informationsvolumens zu erreichen.

Schlüsselwörter: Smart Working Site, Multi Working Machine Pathfinding Algorithm, Multivariable Time Series Classification Algorithm, Mobile Machines Dataset, SLAM, 5G, IEEE 802.11p

# **Contents**

# **Acronyms and symbols**

#### **Acronyms**

#### CRDNN A combination of CNN, RNN, and DNN

#### CSMA/CA

## **Symbols**

#### **Path Planning for Machines Fleet Management**

#### **SLAM for Machines on a Smart Working Site**

#### **Motion Prediction of Manned Working Machines**

#### **Wireless Communication System**

d Distance between transmitter and receiver fc Correction factor for IEEE802.11p analytical estimation method Gr Receive antenna gain

# **1 Introduction**

Recent progress in Artificial Intelligence (AI) and Internet of Things (IoT) makes me believe that the traditional construction and mining site can be extended with an autonomous system or at least an assistant system, for the purpose of increasing productivity and safety performance, as well as reducing the cost for projects.

In this thesis, I focus on the fleet management logistic problem regarding productivity and safety on smart working sites. Currently, because of a lack of effective cooperation among individual machines, heavy machinery wastes a lot of time waiting for each other, and conflicts exist. Therefore, the total number of affordable construction machines within a working site is limited, and the overall productivity is less satisfying. The basic idea is to solve the moving conflicts inside of the working site so that more machines can be invested in performing tasks simultaneously and thus significantly improve productivity. A persuasive instance to show the benefit of introducing the logistic solution into the working site is the construction site for the famous hospital, namely Huoshenshan, the project in Wuhan during the coronavirus outbreak in 2020. By investing an extraordinary amount of working machines and human cooperators and manually coordinating the machines to avoid conflicts among them, the construction project was finished at an unprecedented speed. Apparently, the economic cost of running such a construction site can be quite expensive due to the salary for experienced workers. Also, since the logistic problem is Non-deterministic Polynomial-time hard (NP-hard), computer algorithms can better perform a series of optimization objects, such as shorter moving distance and realtime performance. In light of that, I try to use AI to replace human decisions in the working site and utilize IoT technology to share the information among the participants seamlessly. Compared to the proposals for an individual machine, which usually maximally increase up to 50% performance, fleet management solutions show a potential to improve the productivity of a working site several times.

The thesis presents a series of approaches contributing to the machines cooperation strategy, machine motion prediction, site visual monitoring, and site wireless communication. Then, by combining these cornerstone technologies systematically, I demonstrate the blueprint of the future working site benefited from AI and IoT technologies.

## **1.1 Problem and Goal Statement**

Apparently, the concept of smart working needs a system solution and thus cannot be solved with only one approach. To make it closer to reality, serious technical difficulties should be overcome. In particular, I focus on the following research questions to achieve AI-based fleet management:

I try to answer the aforementioned questions with cutting-edge AI and IoT technologies, which show the surplus human performance capability and thus have become the technological waves in the second decade of 21th century. Since the machines inside of a large working site can be diverse due to their various function, a fully autonomous system for all the machines in the working site is still very challenging; thus, I endow my AI and IoT system with the ability to cooperate with the human being.

While the individual ideas to increase both productivity and safety whereas reduce the cost of projects have been intensively tackled from either management's or technical view, the improvement of the entire working site by an isolated technology is limited. I conjecture that it might because a working site is a complicated system and thus need concerted efforts. In this thesis, I first advance the current SOTA technologies for the individual aforementioned research question and then find the appropriate configuration to show the benefit of smart working site as a whole system.

## **1.2 Applications**

I highlight two critical scenarios where the proposed concept smart working site shall be adopted.

In mining site and construction site, there is a high demand for the highly automated and coordinated fleet management from both economic and safety views. Furthermore, since the machines usually operate in a closed area at slower speeds and untrained pedestrians are already kept out of the construction site, it is easier and safer to automate their driving. The unmanned construction and mining site brings enormous benefits regarding safety, productivity, and labor perspective:

• Safety Benefit: Since 1900, over 100,000 coal mine accidents have taken places in the US [1]. In China, the number of deaths in coal mining accidents exceeded 2,000 each year from 1993 to 2010, whereas the peak occurred in 2002 with approximately 7,000 deaths [2]. The coal miners are highly exposed to coal dust and toxic gas, which results in a higher rate of ischemic heart disease and workers pneumococcus [3]. The safety problem can be significant eliminated by unmanned construction and mining site.

## **1.3 Contributions**

The contributions of this thesis are as follows:

while outperforms the SOTA solution [5] in terms of generalization ability, thanks to its faster transfer learning capability.

## **1.4 Thesis Outline**

This thesis is structured as follows: Chapter 2 shows the current contributions and developments on smart working site. Then, Chapter 3 presents the multi workingmachine pathfinding algorithm that is employed. Chapter 4 details the SLAM technologies are used for acquiring the map information and machines' location in the working site. Afterward, Chapter 5 shows the working process recognition through multivariate time series algorithms to predict machines' motion. Followed by Chapter 6, the visual monitoring system is described. Finally, Chapter 7 illustrates the wireless communication system of the working site. Conclusions and outlooks are drawn in Chapter 8.

# **2 Background Knowledge of Smart Working Site**

This chapter discusses some previous contributions in smart working sites with respect to the existing literature. I begin with a brief overview of the novel technologies and ideas on the smart working site here and go deeper at the beginning of the following individual chapters.

## **2.1 Concepts and Consensus**

Smart working site is a novel integrated management and automation model for working sites, which is a high integration of AI, IoT, and traditional construction industry. It takes full advantage of the emerging information technologies such as mobile internet, Artificial Intelligence of Things (AIoT), cloud computing, big data, and focuses on key factors such as people, machines, materials, methods, and the environment to completely change the interaction and working mode on a working site. Obviously, my research also serves the establishment of the smart working site.

In general, construction project management manages construction components, such as workers, materials, and construction machines, aiming to achieve construction objectives quickly and well. Managing a construction site is to make a series of decisions across construction processes utilizing available information and knowledge [6]. The main objective of information management is to support decision-making by ensuring that accurate information is always available at the right time in the right format to the right person [7]. In recent years, there

Figure 2.1: An example of a smart construction site illustrated by Komatsu.

have been many studies on how to afford decision-makers with precise, timely, and well-organized information, e.g., exploiting Building Information Modeling (BIM) [8, 9]; by adopting Information and Communication Technologies (ICT) such as Auto-ID [10, 11, 12, 13], and sensing technology [14, 15].

However, due to the complexity and diversity of construction projects and the increasing demand for high-quality engineering, the decisions made by human beings are considered as more and more unreliable with the explosive growth of information, especially compared with the decision made by or with the help of advanced AI technology. Problems are often manifested in product quality defects, overtime, and over budget, caused by insufficient information, cognitive ability, and time. These problems afflict engineers with traditional management techniques before the AIoT era.

Fortunately, thanks to the tremendous progress and profound influence of AI and sensor technology, Construction Engineering and Management (CEM) is experiencing a rapid digital transformation. With more available tools, both academia and industry have proposed their novel concepts to realize the construction industry's refined management methods and automation. Obviously, the development of smart construction sites is inseparable from the promotion of information technology. It is noteworthy that most researchers adopt the BIM system as the backbone to build up their contributions. Nowadays, the smart construction site is an integration of comprehensive intelligent systems. The interaction of physical space and cyberspace makes the advantages of smart construction sites be fully demonstrated. From the overall perspective, the management and automation of smart working sites determine a construction project's quality. In addition, compared with traditional construction environments, the smart construction site allows construction quality to be fully supervised. The communication between the staff will become easier and more straightforward, which saves working time, promotes work efficiency, and meanwhile ensures the quality of construction projects. Besides that, smart construction sites have become an indispensable and essential component of safe production. Through various monitoring settings installed at the construction site, as well as a more comprehensive intelligent monitoring and prevention system, it can better make up for various omissions in traditional management work.

The characteristic of CEM can be concluded as uniqueness, labor intensive, high dynamics, complexity, and uncertainty. These literature [16, 17, 18] indicate the basic requirements and the consensus of future working sites are following. First, the problems should be prevented before they actually occur. For instance, the digital twins' concept depicts a cyber-physical system where the digital model offers the simulation results to the physical model that inspection data is collected. Analogously, cloud VR/AR solutions realize more interactions between the cyber and physical worlds. Also, the information shall be shared and make the process more transparent through AIoT and blockchain technology. Consequently, the BIM model can be updated timely whenever the physical model is changed. Moreover, the process is expected to be supervised by using smart robotics, e.g., unmanned aerial vehicles. Last but not least, the AI system shall visualize the most important and concise information to the human decision-makers with data mining tools or substitute the human decisions directly with the advanced-AI decisions in some cases. Here I found some applications using Natural Language Processing (NLP) to retrieve vital information from the reports for the human decision-maker.

## **2.2 Current Applications and Challenges**

Besides the research topics creating improvements in the equipment systems, including increased engine efficiency, reduced greenhouse gas emission, improved electro-hydraulic control, information technology has attracted more and more attention in the last two decades. Especially, the triumvirate of IoT, AI, and cloud technologies offers new opportunities for the development of new applications on smart construction sites [17], shown in Fig. 2.2.

Figure 2.2: The critical topics for smart working site summarized by Štefanič [17] and us.

For example, in 2006, Lundeen developed a marker-based pose estimation system for excavators in order to determine the three-dimensional positioning and orientation of the trencher [19]. Later, a novel system was described by Turkan that integrates 3D object recognition technology with schedule information into a combined 4D1 object recognition system focusing on progress tracking in 2011 [20]. Tested on a comprehensive field database acquired during the construction of the structure of the "Engineering V Building" at the University of Waterloo, this system demonstrates a degree of accuracy for automated structural progress tracking and schedule updating that meets or surpasses manual performance. In 2016, Ren and Wu developed a realtime automated anti-collision system that can warn crane operators about potential collisions and then automatically implement collision-avoidance strategies [21]. One advantage of this system is that it does not require additional devices and can be installed in existing crane controllers. In the same year, another novel system was proposed by Liu that combines inclinometer, laser ranging sensors, and wireless communication technologies to monitor lift-thickness during highway construction [22], and Braun presented a concept for an automated comparison between the actual state of construction and the planned state for the early detection of deviations in the construction process [23]. In this concept, the actual state of the construction site is detected by photogrammetric technology. Concretely, dense point clouds are generated by the fusion of disparity maps created with Semi-Global-Matching (SGM). These are afterward matched against the target state provided by a 4D Building Information Model. Also in 2016, based on the deviation between the optimal route determined by extracting nodes from BIM and the actual route of a laborer collected from the Real-Time Location System (RTLS), Kim proposed an automated hazardous area identification model to improve the efficiency of safety management [24].

As we can see, the contributions about the smart working site demonstrate many benefits and are proved to be promising approaches to improve the current daily working site. However, they are not preferable for commercial solutions until 2020, i.e., most working sites in reality are still quite traditional. I conjecture that it may because that these contributions do not improve productivity several times. Most of the current information technologies and corresponding management in

1 Besides the x,y,z coordinates denoted by the first three dimension, the 4D model describes the schedule information using the fourth dimension.

Figure 2.3: Selected works about smart working site [14, 25, 20, 23, 22, 16, 24, 26, 27].

the earthmoving industry address real-time tracking and productivity estimation of the equipments rather than productivity improvement. As a result, these technologies are considered as incomplete solutions for some end customers, e.g., construction contractors. For instance, productivity estimation function might be challenging to be accepted by some engineers if there are not corresponding productivity-increasing methods. In fact, the McKinsey Global Institute (MGI) analysis found that the construction industry was among the least digitized industries in the total economy, and the annual productivity growth over the past 20 years was only a third of total economy averages2 . Hence, improving productivity is a very urgent and important issue in the future.

Notably, the world-famous Huoshenshan construction project in Wuhan demonstrated the possibility to significantly boost construction productivity3 by means of investing in a large number of machines. In light of that, I explore the approach to increase the number of machines in a working site and solve the cooperation and safety problems among machines and workers utilizing AI and IoT technologies in this thesis.

2 McKinsey: The next normal in construction (2020)

3 To avoid the misunderstanding, productivity is defined as the efficiency to execute construction projects in this thesis. Although there are no comprehensive analysis of how much faster is the Wuhan's construction site, ultra-rapid is the most well-known feature for this site [28, 29].

# **3 Path Planning for Machines Fleet Management1**

Multi working-machines pathfinding solution enables more mobile machines simultaneously to work inside of a working site so that the productivity can be expected to increase evolutionary. To date, the potential cooperation conflicts among construction machinery limit the amount of construction machinery investment in a concrete working site. To solve the cooperation problem, civil engineers optimize the working site from a logistic perspective while computer scientists improve pathfinding algorithms' performance on the given benchmark maps. In the practical implementation of a construction site, it is sensible to solve the problem with a hybrid solution; therefore, in my study, I proposed an algorithm based on a cutting-edge multi-pathfinding algorithm to enable the massive number of machines cooperation and offer the advice to modify the unreasonable part of the working site in the meantime. Using the logistic information from BIM, such as unloading and loading point, I added a pathfinding solution for multi machines to improve the whole construction fleet's productivity. In the previous studies described in Section 3.2, the experiments were limited to no more than ten participants, and the computational time to gather the solution was not given; thus, I publish my pseudo-code, my tested map, and benchmark my results. My algorithm's most extensive feature is that it can quickly replan the path to overcome the emergency on a construction site.

1 All the figures, text, and results of the presented work in this chapter have been published in our publication [30]. My contribution to the paper is summarized as 100% in terms of conception and methodology, 90% of literature review, 90% of code, 60% of results visualization, and 95% of formulation.

## **3.1 Introduction**

Although many achievements in construction machines with respect to productivity and safety, humans' pursuit of even higher productivity and better safety never stops. In the past decades, civil engineers and construction-industry-related software engineers introduce the BIM [31] as a powerful tool to increase productivity and safety performance whilst reduce the project cost by means of digital technology. In general, BIM provides the 3D or more than 3D model of the construction projects and even the installation sequence of the components to avoid mistakes during the construction stage. With the maturity of BIM, this software and process are adopted for many large and especially complicated construction projects worldwide [32] since the mistakes during the real construction process cause much more severe consequences than those in virtual engineering. Also, BIM is considered as a lifelong software, contributing to not only the early construction phase [31, 33] but also the time after the construction projects are finished [34]. Despite to the preliminary cost for training corresponding laborers and model building in a computer, BIM is a necessary tool for at least large construction projects becomes a consensus. However, although current BIM software defines the start and end points where the material should be transported, concrete paths guiding the trucks to accomplish the goal are not given. Or more generally, an algorithm that determines the paths of the participants in the working site so that they can move to their destination without collision and hesitation is still developing.

As shown in Fig. 3.1, one motivation to combine these path planning algorithms with BIM can be described as determining the construction machines' travel path so that the machines can be expected to move faster and denser without hesitation. To accomplish higher productivity and better safety in the path planning fashion, some researchers calculate and display efficient paths by a given construction site [35, 36] whereas others optimize the construction site layout considering the high productivity of the path [37, 38]. However, most of the current solutions about path planning on a construction site are mainly focusing on individual units, i.e., the interaction and path conflict of the different machines or other agents

Figure 3.1: Overview of the smart construction site concept. I introduce to use artificial intelligence to unify the scheduling of construction machinery on construction sites. Maximize the use of construction machinery on specific construction sites by avoiding conflicts among construction machinery and thus ultimately improve the productivity of the construction sites.

are ignored. Consequently, the working sites' spatial and time utilization is limited. Inspired by warehouse logistics solution [39], where a lot of robotics are working with the commands from path planning algorithms in the meantime and thus achieve considerably higher transport efficiency as a fleet, I envision the Multi-Agents Pathfinding (MAPF) solution can also provide evolutionary to the construction industry. A persuasive instance to show the benefit of introducing MAPF solution into a working site is the construction site for the famous hospital, namely Huoshenshan, in Wuhan during the coronavirus 2019 outbreak. Invested an extraordinary amount of working machines and human cooperators, the construction project was finished at an unprecedented speed, through only manually coordinate to avoid conflicts among machines. Apparently, the economic cost of running such a construction site can be quite expensive due to experienced workers. Also, since the MAPF problem is NP-hard, computer algorithms can surely have a better performance concerning a series of optimization objects, such as shorter moving distance and realtime performance. In light of that, I propose a MAPF solution for working machines as an extension of the BIM system so that more machines can work simultaneously and thereby achieve better holistic productivity.

The aim of this chapter is to extend the current BIM software with AI pathplanning algorithms in order that more machines can work simultaneously since the paths are calculated to avoid collisions. Because the start points and endpoints can be given directly in the BIM system, I devote myselves to the implementation level, i.e., how exactly the machines should achieve their goal set given by the BIM system. I envision that the case in Wuhan will be a normal case in the future by utilizing AI and IoT [40, 41] technology.

The main contributions of this chapter can be sum up as the following points:

The rest of this chapter is organized as follows. Section 3.2 briefly introduces the prerequisite and background knowledge in fields of BIM, path planning methods for construction machines and robotics to understand this chapter quickly. Next, the existing problems are illustrated in Section 3.3. After that, in Section 3.4, I describe the setup of my MAPF approach, including the map to give the start and goal positions, low level for individual pathfinding, and high level for conflicts solving. Then, I show the experiments setup. Followed by Section 3.6, I show my approach's performance by testing on some maps based on real construction sites. Finally, Section 3.7 summarizes the advantages of my approach, and Section 3.8 gives conclusions and envisions the outlook.

## **3.2 Related Works**

### **3.2.1 Building Information Modelling (BIM)**

BIM is a 3D model-based information management process in the field of Architecture, Engineering, and Construction (AEC) that facilitates efficient design and construction processes and inter-organizational collaboration [42]. There is a lot of BIM-based software: Autodesk Revit Building (Revit), ArchiCAD, Bentley, and SolidWorks [43]. By building the whole project virtually before physical construction begins, construction sequencing is determined, including material ordering, fabrication, and delivery schedules for all building components, etc. Therefore, conflict, interference, and collision are avoided in the early stage, contributing to improved site efficiency and reduced cost [44]. As the function inside BIM increases, such as scheduling, virtual reality [45], and logistic management [42], it has been extended to 4 or more than 4 dimensional model. Nowadays, the research about BIM is prosperous. Combining AI and IoT into the BIM system is considered the next potential boom for BIM systems. Survey papers about that can be found in [46, 47].

In order to automate the whole construction site and compute the optimal path for the heavy machines, logistic information is vital, which demonstrates which materials should be placed in which location at which time in the right quantity. Logistics management in construction involves the strategic storage, handling, transportation and distribution of resources, as well as planning of a building site's layout [48]. Whitlock has proposed a desktop approach to adopt BIM for construction logistics management [42]. Such logistic information, for instance, unloading points, on-site arrangements-logistics layouts, which are generated at the outset of the pre-construction process, can be used as the input data for the path planning.

#### **3.2.2 Path Planning for Construction Machines**

On a construction site, there are usually multiple machines working simultaneously in a definite area with given assignments. Therefore, coordinated construction logistics can definitely increase productivity, decrease material usage, and guarantee workers' health and safety [49]. To date, there is plenty of researchers proposed their solution for the logistical problem inside construction sites at diverse levels, i.e., path planning and motion control. In this section, I summarize the previous research about moving paths inside of construction sites.

In the construction industry, the earth-moving sector is among the pioneers in adopting new sensing and information technologies [16], such as bulldozer [50, 51], and grading machines [52]. Given two points A and B on a construction site, the objective is to determine the shortest path from A to B maintaining a safe distance from obstacles. The approach proposed by Kim is a path-planning method for a mobile construction robot to find a continuous collision-free path from the initial position of the construction robot to its target position by improved Bug-based algorithm [53]. The algorithm can work with the disturbance of static and dynamic objects. Obviously, the performance of the approach is based on the accuracy of the sensors. At that time, the methods to acquire site information were still immature. Hence, the spatial model supporting path planning in a partially known and partially unknown environment was brought forward by Lee. Accordingly, the spatial model provides the domain for finding an efficient path on a construction site through the use of an algorithm that combines a shortest path algorithm and a dynamic path-planning algorithm. This approach differs from existing path-planning approaches that assume the construction site is totally known or totally unknown. Thus, problems associated with managing a changing construction environment and ignorance of designated roadway networks are overcome [54]. In the same year, Soltani has compared the performance of different methods for the path planning inside construction sites [55], such as Dijkstra [56], A∗ [57], and Genetic Algorithm (GA) [58], by evaluating comprehensive multi objects, e.g., site layout representation, distance formulation, hazard zone modeling, and visibility calculations. Also, the author points out the use of Closed-Circuit TeleVision (CCTV) cameras should be considered to enhance site security [55]. Although the simulation results show that the GA has the best performance and the other two algorithms have quite similar results, I conjecture that it might be ascribed to their maps, which are quite easy and do not include difficult obstacles such as bottlenecks. Fairly recently, Song tried to integrate some path planning algorithms into BIM system [35]. The basic idea is to determine the path to transport the materials at the very beginning phase, i.e., the construction site design phase. Also, the study verified the demand for the introduction of path planning by survey questions. The shortcoming of this approach is that the interaction of other participants inside construction sites during the construction project was not taken into account.

So far, the aforementioned studies focus on a path for one machine inside the construction site. In contrast, the following research shows solutions for multi machines working simultaneously on a construction site. An influential study about the path planning on the construction site is from Cheng published in 2012 [59]. The objective of the paper is to provide the n best and safest paths between two points in a work area while maintaining a safe distance from identified obstacles. Here the approach proposed in the paper uses Dijkstra algorithm and solves the path of different participants in sequence. Due to the limited recognition distance of ultra-wideband sensors, the usage of this approach might not suitable for huge working sites. Also, since the paths of agents are calculated one by one, the computation efficiency is not ideal from today's point of view in 2020. 4 years after Cheng's study, the research from Bohacs shows the difficulties of path planning for a construction site. Also, they use A∗ as basic and develop an algorithm to let limited machines can cooperate without collision [60] within a small map. Concretely, they showed the demo about 3 machines in a 10 × 10 grid map. As the flow chart in their paper shows, the algorithm depends on the condition statements. As a result of that, it cannot perform with all the dispensable computation effort of the computer.

As the development of information technology, Štefanič provides an overview of emerging smart construction applications in areas such as construction monitoring, construction site management, safety at work, early disaster warning, and resources and assets management [17]. Also, Tumer describes the future construction site utilizing industry 4.0 [61]. Without a doubt, a future working site should be fully benefited from AI [62, 63, 64], automation technology [65], Simultaneous Localization and Mapping (SLAM) [66], and IoT [67, 40, 68, 41]. Therefore, the uncertainty degree of construction sites is reducing, and thus I consider the construction sites as known in my research.

Based on the strict literature review, A∗ is the most developed and latest algorithm to solve construction sites' path tasks. It combines both step-cost calculation from the Dijkstra algorithm and feedback step from the genetic algorithm. However, as the number of machines increases, naive A∗ might not be suitable to solve the MAPF problem due to algorithm complexity. Thus, it is necessary to explore the SOTA solutions in the field of mobile robotics in order to find a more appropriate solution.

### **3.2.3 MAPF for Mobile Robotics**

For a single agent, the planning task can be described as finding the lowest cost from the starting point to the targeting point. By using Heuristic Search, e.g., A∗ Algorithm, such problem can be better solved. However, naive A∗ Algorithm only considers that all the obstacles are static, which is the ideal assumption in the pathfinding problem [69]. In contrast, to solve the MAPF problem, the other participants in the map must be considered, i.e., the dynamic obstacles also affect the optimal path of an individual agent.

MAPF for mobile robotics is both a well-studied and dynamic developing topic. The usage scenarios of MAPF and their corresponding algorithm are diverse, such as warehouse [39], computer games [70], and autonomous driving in intersection [71]. Until the end of 2020, although the concept of reinforcement learning is attracting more and more attention, current influential research about the shortest path and MAPF is mainly based on graph theory. To date, there is no universal solution for all kinds of pathfinding problems; thus, algorithms with different time and space complexities are proposed [72].

Based on the survey paper from Felner [73], it is known that no algorithm dominates all others in all circumstances. There is a tradeoff between high-quality path solutions and realtime performance. The mainstream MAPF solution can be classified into search-based solvers and rule-based solvers. The former intends to find the best solution or near-optimal solutions, whereas the latter can run much faster, however, produce far away from optimal solutions. Of course, some compromised solutions combined two ideas together, namely, hybrid approaches. Another type of solution, namely reduction-based optimal solvers, focusing on reducing MAPF problems into some problems, such as the Constraint Satisfaction Problem (CSP), with a well-known solution. Since this approach usually only aims at the makespan tasks, I will not go much deeper in this approach. To date, the most influential solutions for MAPF are Conflict Based Search (CBS) and its variants due to their widely used real-world applications.

Sharon proposes Conflict Based Search (CBS), combining both advantages from coupled and decoupled approaches. Although the pathfinding process is strictly single-agent searches, it can guarantee to offer optimal results, unless the variant that deliberately provide a suboptimal solution for the purpose of realtime performance. As the authors introduce, CBS adopts a two-level structure, where the high-level search can be described as a Constraint Tree (CT), including every constraint. Then, the lower level finds the concrete path for each agent individually with the information from the higher level. The brilliance of this design is that the search process is not more exponential in the number of participants but exponential in the amount of conflicts encountered during the pathfinding process.

Since CBS tries to find the optimal solution and thereby causes a relatively longer runtime, in improved CBS algorithm [74], Boyarski summaries two methods to reduce the runtime. Concretely, it firstly adopts the Meta-Agent CBS (MA-CBS) [4], which merges multi-agents together and handles them as a large agent. Moreover, it uses bypass improvement, which encourages one of the agents to find an alternative path instead of performing a split at the high level. As mentioned by Boyarski, the bypass concept successfully avoid the unnecessary generate the new nodes in the CT [74]. Since the bypass concept only tries to find the solution from the path with the same cost as the one that shall be replaced, the optimality cannot be harmed. In the high-level search, Felner suggests adding heuristics into CBS so that the conflicts are not arbitrarily chosen [75]. After that, Li found the improved heuristics to guide the high-level search [76]. Also, She introduced the CBS with disjoint splitting [77]. The main contribution of CBS with disjoint splitting is the novel terminology of positive constraints forcing the ai to be at v at timestep t. In this fashion, CBS with disjoint slitting reduces the amount of unnecessarily expanding the CT. In addition, some improvements, such as Lazy CBS, which avoids the behavior that CBS resolves the same conflicts between the same pairs of agents many times owing to lack of connection among subproblems [78]. Hönig proposed an approach called Conflict-Based Search with Optimal Task Assignment (CBS-TA) [79]. The improvement is mainly because it creates the forest on demand. Solving MAPF optimally is proven to be NP-hard, so CBS and all other optimal solvers do not scale up. Alternatively, Barer proposed a suboptimal variant of the CBS algorithm [80] so that the problem can be solved suboptimally but much faster.

To sum up, naive CBS is an optimal pathfinding solution that is based on graph theory. The time consumption of the algorithm mainly depends on the conflicts occurring among the agents since they increase the nodes in the high-level tree. Its performance on bottlenecks and corridors is better, whereas the performance on open space can be worse than enhanced A∗ . Thus, CBS's variants are focusing on reducing the nodes in the CT and therefore let CBS has a higher success rate in general. Note that in this chapter, I use the same terminology as Stern's research to avoid misunderstanding; however, some mathematical descriptions might be adjusted.

### **3.3 Problem Statement**

Although the path planning problem has been attracted engineer's attention, the proposed method is quite tricky to be used in a large construction site with many machines. Theoretically, given a 4 neighborhood movement model and an undirected graph G(V, E), the branching factor should be estimated as (E/V ) k and the search space is V k if k machines should be planned. For 20 agents, considering that a normal working site with 500m×100m where 50×10 cells are needed, the search space goes to (50 × 10)20 = 9.54 × 1053, which is unsolvable within acceptable duration for a real application even if the top CPU in 2020 achieving approaching 200 GFLOPs is used. Thus, using the traditional methods to solve the cooperation task is still challenging. Also, some algorithms are based on replanning the path if agents encounter a head-to-head position. However, these methods require excellent perception capability and limit the movement velocity of agents.

On the other hand, CBS based solution treats all the objects equally. Concretely, each robotics has the same capability, the cell on the ground is assumed as equally challenging to be overcome. However, it does not hold true in a working site; some machines should be assigned a higher priority, and some paths are much easier to pass. For instance, larger machines are usually more challenging to control their velocity. Also, stopping on a slope is much dangerous than on the flat ground. Hence, in my study, I further develop the original CBS so that it can deal with plenty of priority problems in a real working site. Also, the computational time should be much shorter to handle emergence.

### **3.4 Model Building**

Nowadays, with the development of SLAM regarding visual recognition, IoT, and satellite technology, the uncertainty degree of construction sites is reducing. Thus I consider the construction sites as known in this chapter. In most instances, the MAPF solution can be evaluated twofold. The first one is sum-of-costs

Figure 3.2: A grid map with terrain weight based on a real construction site, drawn by Liu [81]. In this map, the green, orange, and red grids demonstrate the easy, normal, and difficult to pass terrain, separately. The blue cells denote the place where it is considered impossible to pass, such as occupied by the obstacles. On a real construction site, the obstacles can be the place to store construction materials temporarily.

which describes the accumulative cost of all the agents. Such costs can be time consumption, fuel consumption, or some other objective goals. The other way to analyze the performance of MAPF solvers is makespan, indicating the maximal time the last goal has been achieved. Obviously, unlike the robotics in warehouses, working machines perform a relatively long period to do their duty after they have arrived. Thus, makespan is not so vital compared to sum-of costs in the field of construction or milling machines. Consequently, I adopt sum-of-cost as the evaluation criterion rather than makespan.

For working site MAPF problems, the problem can be described as given a graph, G(V, E), and a set of k agents labeled as a1 . . . ak. Each agent ai has a start position si ∈ V and goal position gi ∈ V . Based on the practical conditions in a working site, I consider vertex conflicts, edge conflicts, and swapping conflicts as unacceptable conflicts, whereas following and cycle conflicts are allowed in my study. Formally, the unacceptable conflicts are described as,

$$\begin{cases} \pi\_i[t] = \pi\_j[t], \\ \pi\_i[t] = \pi\_j[t+1] \cup \pi\_i[t+1] = \pi\_j[t] \end{cases} \tag{3.1}$$

where πi and πj are the single-agent path for ai and aj at time step t, correspondingly. The first equation shows the vertex conflict whilst the second equation describes the swapping conflict. Apparently, edge conflict is a union of vertex conflict; thus, no additional equation is needed. Intuitively, π1[2] denotes the location of the first agent at the second time step.

#### **3.4.1 Multi-Layer Grid Map**

Unlike a standard graph problem which adds the weight on the edges, I add the weight directly on the grids. This is mainly for three reasons. First and foremost, the machines usually occupy a relatively large area and thus should not be modeled as a simple vertex and ignore their geometry. Also, most previous studies in the field of construction machines used the grid-based map. To guarantee compatibility, I tend to use a similar solution since no approach from them obviously outperforms the other one. Last but not least, if weights are applied on edges, it becomes challenging to penalize the waiting process.

Inspired by the research from Fankhauser [82], a map can include many layers to store different types of data information. Obviously, the map information should be saved in BIM system so that the path planning process can be done. In the previous study [66], Xiang developed a realtime map plotter of the construction site according to ground condition, offering multi-layer grid-based maps, which divide the environment into uniform cells. Moreover, maps can also be gathered by Lidar or cameras provided depth information installed on a drone or on the ground.

Fig. 3.3 illustrates the multi-layered grid map concept, where each cell data is stored on the congruent layers. In many construction projects, since resistance and grade of the road are the most of importance information for the construction

Figure 3.3: An example of multilayered grid maps. My approach depends on multilayered grid maps to offer data of different types of information to make the best path. Concretely, every grid saves a 1\*3 matrix, including location information and the corresponding terrain information. The map, which can be visualized in the BIM system, is saved as a 2\*m\*n\*3 tensor, where m is the max displacement in the x-direction, while n is the max displacement in the y-direction. In case a place is unknown, it will be marked as NaN to denote the uncertainty of the regions and be treated as obstacles.

Figure 3.4: Detail description of a layer in the grid-based map.

machines, I show a two layers grid map as an example. Concretely in my study, the map is divided into small cells, whose resolution is 10 meters per cell to cover the geometry of the vehicles. In practice, GPS/IMU based Kalman filter algorithms can be used to locate the mobile machine on the construction site. To describe the ground condition of construction sites, I use the value of each cell to represent the information of the ground situation. In Fig. 3.4, a layer that holds the data of a grid-based map is shown. Apparently, although I only demonstrate the map with two layers, it is relatively easy to extend the third layer in case more information should be taken into account for the path planning because I can simply add the weights together.

#### **3.4.2 Lower Level Search**

The concept of CBS does not limit the lower-level search algorithm. In addition, since negative weight cannot happen in 3D space, I believe that Dijkstra and A∗ can work well for individual shortest path search. In light of that, I use best-first search2 . Also, to accelerate my algorithm, I limit the moving direction of an agent to 4 and thus reduce the branching factor to 5, including wait, instead of 9 or more. In order that I can get the optimal solution, I set the heuristics smaller than the real distance since weights are considered. Because grid map is used, I use Manhattan distance as the base of the heuristics to guide the search.

In the lower level, the algorithm searches the best path of individual agent based on the estimated cost of through current vertex to the goal, formally,

$$f\_{f,r}(n) = \sum\_{i=1}^{p} W\_{L,i} \cdot g\_{i,f,r}(n) + h\_{f,r}(n) \tag{3.2}$$

where f(n) is the estimated cost from its source through current vertex n to its goal, gi denotes the real cost from the source to the current vertex n considering the ith weight-grid map, and h denotes the estimated cost from the current vertex

2 The concept that searches the most likely region first.

Figure 3.5: The basic requirements of the path planning algorithm. As shown in the left subfigure, the vehicle should take the lowest-cost path to reach its goal. The middle subfigure shows that the vehicle with lower priority should wait until the vehicle with higher priority pass through if there is no other bypass possibility. Last but not least, the right subfigure indicates that a good path should not be too close to dangerous objects.

n to the predefined goal based on Manhattan distance. WL is the weight for the specific layer. The index f and r show the estimated cost is forward or backward.

Apparently, planning the best path for machines to reach their goal is a multiobjective task. Generally speaking, the evaluation criterion can be divided into subjective and objective criteria. Obviously, the objective criterion demonstrates the objective criterion of the planned path, especially the terrain which can affect safety and efficiency. As some roads inside the construction site can be built with asphalt, so is considered better road conditions than some road made of sand. Consequently, the cost of passing different routes is different. Besides that, the road slope should be taken into account since waiting on a steep hill is much more dangerous than staying on flat ground. Therefore, I introduce multi-layer to record the individual characteristics of the terrain and plan the best path based on them. Concretely, a construction site map is divided into a series of cells and layers, according to different criteria. The weights of individual cells in one layer are saved as shown in the following matrices.

$$
\tilde{W}\_{m,n} = \begin{pmatrix} w\_{1,1} & w\_{1,2} & \cdots & w\_{1,n} \\ w\_{2,1} & w\_{2,2} & \cdots & w\_{2,n} \\ \vdots & \vdots & \ddots & \vdots \\ w\_{m,1} & w\_{m,2} & \cdots & w\_{m,n} \end{pmatrix}
$$

In contrast, the subjective criterion may not harm the whole system's actual performance; however, it has an impact on people's psychology. For instance, a crane or some other dangerous objects, such as a power station, on a working site should be protected, and the situation that the mobile machines unnecessarily approaching them should be avoided. As we know, even machines did not involve in an accident, getting close to a dangerous object will be stressful for site managers and indicating a potential risk. To address this problem, I add g3 to penalize the machines for occupying the areas surrounding these special objects, see Eq. 3.3,

$$g\_3(n) = \sum\_{o=1}^{r} \frac{C\_o}{|(X\_n - X\_o)| + |(Y\_n - Y\_o)|}\tag{3.3}$$

where [Xo, Yo] is the position of the objects which should try to avoid being approached, Co denotes the intensity.

#### Algorithm 1 Bidirectional A∗ Algorithm at Low Level to Speed up the Searching Process

Input: G(v, t), s, ˜ g˜ from predefined map information in yaml, original from visual or Lidar recognition

Output: P ath, dshortest Initialisation :

```
LOOP Process
5: while OpenSet[0], OpenSet[1] not empty do
6: vc ← ExtractM in(dist), forward otherwise distR
7: OpenSet.remove[0](u)
8: if neighbor(u) = valid then
9: if u not in ClosedSet[0] then
10: g[0][u]tmp ← g[0][v] + StepCost
11: end if
12: if neighbor not in OpenSet[0] then
13: OpenSet.append(u)
14: else if g[0][u]tmp > g[0][u] then
15: continue
16: end if
17: CF[0][u] ← v
18: pif ← |
              −−−−−−−→
              d(v) − d(s)|, pir ← |
                                 −−−−−−−→
                                 d(t) − d(v)|
19: hf ← (pif − pir)/2
20: hr ← −pf
21: f[0][u] ← g[0][u] + hf , if forward otherwise hr
22: end if
23: if u in ClosedSet[0] then
24: break
25: end if
26: ClosedSet[0].append(u)
27: repeat symmetrically for v
                           R as for v, where OpenSet[1] and ClosedSet[1]
     should be used
28: end while
29: distance ← ∞, ubest ← None
30: for u in ClosedSet[0] + ClosedSet[1] do
31: if dist + distR < distance then
32: ubest ← u
33: distance ← dist[u] + distR[u]
34: end if
35: end for
```

Figure 3.6: Illustration of the benefit of bidirectional search with heuristics. I use squares and triangles to represent the start point and the goal point separately. Here the grey region denotes the whole region of the map, and the green region shows the region that the algorithm must search before finding the shortest path for agents. Apparently, bidirectional search with heuristics cannot be slower than its antecessors.

To accelerate the low-level searching process, I utilize the bidirectional A∗ algorithm with a path return for the initial search. The following Algorithm 1 shows the algorithm's steps, where CF is the dictionary to save the path sequence. Since bidirectional A∗ is not suitable for dealing with the waiting operation, neighbors are valided if the grids are not occupied by obstacles, i.e., ignoring the conflicts with other agents. The index 0 in the algorithm denotes forward, whereas index 1 means backward.

After the initial solution is proposed, I adopt unidirectional A∗ to solve the conflicts among the agents. Also, the priority of the agents is taken into account here. As a conflict occurs, the algorithm will give the order which agents should avoid other higher priority agents. Unlike some other research using hard priority, which might lead to the algorithm become not complete, I adopt the soft priority, namely penalization function, to guarantee the algorithm to find the feasible solution if the problem is a solvable MAPF problem. Since the A∗ algorithm is well known, I only give the part that involves in the priority of the agents, see Eq. 3.4,

$$g[u]\_{tmp} = g[v] + P\_{a\_i} \cdot \sum\_{k=1}^{L} StepCost \tag{3.4}$$

where u is the neighbour point and v is the current point. L is the total layers of the map, and Pai is the priority value of the agent i.

#### **3.4.3 Higher Level Search**

Although the papers about CBS and its variants showed the success rate of the algorithms under various specific scenarios, they are performed with a time limit of at least 1 minute. Considering that some machines might not maintain their speed and emergencies may happen, I believe a feasible algorithm for the construction site should give a command to all the participants within 5 seconds even the order can be just wait.

Concretely, I let some machines have priority to move to their goal while the others should wait for a while or find a midway destination in case when the task is too complicated for a realtime response. Or the algorithm suggests to reduce the total mount of machines on site if necessary. Different from the original CBS algorithm, I add four different strategies in case the algorithm cannot find a solution for all agents within 5 seconds. The basic ideas of these acceleration process are reducing the complexity of the task by solving the problem step by step and described as follow,

The individual path will be compared at the high-level search to find out the conflicts among the agents. I use bidirectional A∗ algorithm with the heuristics proposed by [83] to create the initial path of each agent in order to enhance the realtime performance, and afterward utilize unidirectional A∗ to update the individual path of each agent since only unidirectional A∗ can deal with the waiting process. In the BIM system, during the searching process, the algorithm saves the conflicts position and the corresponding agents. As mentioned, in case the algorithm cannot solve the planning problem due to too many conflicts, the algorithm will remove some agents and then replan the paths of the rest agents. In this fashion, I ensure that the known MAPF problems can be solved in a timely manner. If emergence occurs and the algorithm cannot solve the new task in time, I can easily and quickly locate the trouble maker. Apparently, the optimization direction here is not only to ensure a short calculation time, but also to make as many machines as possible move at the same time.

## **3.5 Experiment on Real Working Sites**

As a consensus of the research in the field of graph theory, although a conclusion about a specific map cannot guarantee its effectiveness on another map, the closer the map, the closer the effect. In order to show the benefit of the introduction of MAPF in the mobile machines industry, I validated my algorithm on five typical real working sites. Concretely, they are a relatively open field with 20 or 50 agents, an open field with many obstacles, a two-side working site connected by a bridge or narrow corridor, and a typical mining site. Since the map will also be shown in the path results as background, here I only demonstrate how a map will be processed to give the prior information for successfully pathfinding on the first map and third map to avoid repetition. The maps shown in Fig. 3.2 and Fig. 3.7 are on the same site at different times. Obviously, Fig. 3.2 is the earlier stage while Fig. 3.7 shows the later stage as the construction process proceed since more facilities are there. In my experiments, the dimensions of the proposed maps are 20 × 13 and 17 × 12. For the sake of simplicity, I also assume the velocity of all the machines are constant; however, it is surely easy to achieve the situation that the machines have quite different speed since I can use the fastest speed as a reference and allow the slower agents occupy more than one grid at the same time or vice versa.

Obviously, the faster the project, the faster the construction site changes. This indicates the difficulties of using a pre-calculated path planning for a construction site. In this study, I divided the grids into different regions with respect to whether the road is easy to be passed through, the slope of the road, and whether the place is safe.

Table 3.1: Weight table. The weights I use to describe the complicated terrain of construction sites

Figure 3.7: Map example drawn by Liu [81]. Another map based on a real construction site which has more narrow corridor.

Here I show the solution finding time on CPU core i7 4720HQ@ 2.6 GHz. Because of its reasonable price at the end of 2020, it is suitable for large-scale commercial use. To reduce the randomness, I did the experiments 50 times and gave the average finding time, and the average number of conflicts occur to analyze the conflicts and thus show the rationality of my optimization. Notice that I rounded the numbers to one decimal place if any.

Before I analyze the results of my experiments, I summarize the basic ideas of the algorithm I used. Similar to the original CBS algorithm, my MAPF algorithm also adopts a two-level search, where the upper level finds the conflicts among the agents and the lower level search the best path for individual agents. The lower level finds the path first and sends the initial proposal to the upper level. Afterward, the upper level will check whether the planned path has a or many conflicts with others. In case there are no conflicts, the center commander, an AI system, agrees to the preliminary proposal to become the final solution of MAPF and all agents are allowed to execute this solution. In other cases, if there have some conflicts, the upper level will find out the conflicts and send this information as constraints to the lower level to avoid the conflicts. To generate the initial individual path for each machine faster, I use bidirectional search. And then update the individual path if the upper level finds out a conflict with unidirectional A∗ algorithm. The algorithm tries to modify the solution having the lowest cost, which guarantees the solution to be optimal. In the experiments, I do not assume what the participants are, nor do I assume its working process to ensure the generalization of my method.

## **3.6 Experimental Results**

In this section, I demonstrate the planned path for each map in Fig. 3.8. As we can see, my algorithm successfully finds out the optimal paths considering the priority of the machines, i.e., the path with the lowest cost considering the main criterion, for all the tested maps. The algorithm commands the machines to drive directly to the goal, find a bypass, or just wait for others first to pass through.

In Tab. 3.3 on page 46, I give the computational time to find out the optimal solution. I divide the searching time into initial search and the following update process. In the first phase, the computational time is no more than 0.1 seconds on the tested maps. In case that the MAPF task is easy, i.e., the counteraction and potential conflicts among the agents are rare, the update process can also be done very fast. As we can see, the total duration to get the optimal solution is within 0.2 seconds for the scenario of agents on map 1. However, in other cases, such as the MAPF tasks on map 2 and map 4, although the period to offer the initial path proposal has no significant difference, the total duration is quite different. Concretely, the tasks on map 2 and map 4 need about 6.8 and 10.4 seconds to be solved. For such tasks, the solution can of course be found inside the BIM system before the machines execute their order saved in the schedule file. In the ideal case, the computational time for finding the solution is not the critical thing. However, in a real application, it is normal that the participants do not act on time when something urgent happens; thus, the ability to replan the path quickly is particularly important rather than let all the machines wait in place. As shown in Fig. 3.11 and Fig. 3.12, the update process is a dominant part of

Figure 3.8: The planned path for each map. In the clockwise direction, the subfigures demonstrate the final solutions, including the best path for each machine for maps 1, 3, 4, 5. The left bottom point is defined as the original point (0,0), and the horizontal axis is the first axis. The layout of map 2 is the same as map 1; however, the difference is that there are more agents on map 2. Due to its huge amount of information, I give the planned schedule in Tab. 3.2 instead of using figures.

the whole searching process. Also, with the data shown in Tab. 3.3, comparing the duration of the initial search and the following update process, it is easy to conclude that reducing the update process is the main optimization direction to make my algorithm faster.

In this chapter, I demonstrate the optimization mechanism on the MAPF task on map 2 and map 4 to avoid wordy; however, I confirm the conclusions I make are also in line with the other maps I tested. The results on map 2 and map 4 are shown in Tab. 3.3. The optimization depends on the stage of the construction

Table 3.2: The schedule of MAPF task on map 2. In this task, the algorithm should plan the path for 50 agents

Figure 3.9: The place where the agents intend to pass through and the resulting conflicts on the second map. As it shown from the points allocation, initial conflicts have a great relevance to the conflicts occurring during the conflicts avoidance process. Notice that I did the experiment 50 times and the conflicts shown in these figures are the average number of these 50 experiments.

site. In the early state before the site is built up in reality, engineers have more freedom to optimize. To accelerate the computation, two methods are proposed for the early stage. The first idea is to modify the unreasonable part of the construction sites. In this fashion, the throughput of the construction site can be improved. Fig. 3.9 represents the positions where the algorithm commands the machines to pass through but encounters conflicts with other construction machines. As aforementioned, I consider two kinds of conflicts in this study since they are more in line with the construction site, namely edge conflicts and vertex conflicts, respectively. Fig. 3.9(a) and (c) demonstrate the conflicts found by initial bidirectional search. Since the map is weighted, the best path is usually

Figure 3.10: The place where the agents intend to pass through and the resulting conflicts on the fourth map. As we can see from the points allocation, initial conflicts still have a great relevance to the conflicts occurs during the conflicts update process.

unique; this is partly proved by the fact that the conflicts number found by initial bidirectional search is a multiple of the time I did the experiments. However, the unique best solution increases the possibility of generating conflicts. Taking the MAPF task on map 2 for example, as shown in Fig. 3.9, it is shown that there are three regions that have more conflicts than others. Concretely, they are the region including vertex (16, 8) and (17, 8), the region including vertex (2,7), as well as the region including vertex (17,12) and (16,12). Correspondingly, based on the intended movement of the agents around these positions, the algorithm points out that the vertex (16,9), (17,9), (3,8), and (15,12) shall be modified to have similar characteristics as its surrounding. For instance, the road condition of the vertex (16,9) and (17,9) shall be changed into good condition from bad condition

Figure 3.11: Statistics of the conflicts made by corresponding agents on map 2. Here blue histogram denotes the conflicts found by initial bidirectional search, and the orange histogram shows the conflicts solved while updating the solution with unidirectional search.

Figure 3.12: Statistics of the conflicts made by corresponding agents on map 4.

since their surroundings have good condition. The computational time is then dramatically reduced to about 0.31 seconds and seems to be the most effective method to reduce the solution finding time. The results are demonstrated in Tab. 3.3.

The other idea is to remove the most troublemaker in the MAPF task. As we know, the capacity of each construction site has a physical upper limit. No matter how excellent the algorithm is, too many participants will eventually lead to a decline in overall performance. Compared to the first method, which has a potential drawback that there might be some reasons that optimization of the working site is not always feasible, removing a conflicts-causing agent can be used whenever needed. In the case of the MAPF task on map 2, agent 16 is removed according to Fig. 3.11 so that the computational duration reduces to roughly 0.97 seconds. Considering the holistic productivity is only marginally affected since there are still 49 machines that can work well in the working site, and the shorter duration endows the whole system the capability to deal with the emergence on site, this method is recommended if the construction site cannot be modified.

In the BIM system, I consciously made sure that the computational duration was within an acceptable period. However, this cannot guarantee all the potential conflicts for these MAPF tasks were removed. For the example of the MAPF task on map 2, in case that agent 16 did not catch up with the planned schedule and had a two-time step delay, the duration for replanning the solution for the whole fleet went to more than 5 seconds, even the construction site was modified in the early stage. Since I had already optimized the searching process in BIM so that the duration is shorter than 0.5 seconds and the only thing changed here was agent 16, I could quickly draw a conclusion that the expanding computational time was due to agent 16. Afterward, the algorithm checked the path of other agents and found out a new temporary destination for agent 16 to avoid conflicts. Concretely, the new goal for agent 16 shall be the vertex (15,7), and the computational time was then only 0.42 seconds. Notice that agent 16 is not allowed to stop at its original place since it blocks the only way for agent 17 and 14 to reach their goal. In case that machine 16 totally lost its mobility, the algorithm will ask every participant to stop and wait for the human intervention. In contrast, If it is not on time due to external distributions, it will wait for other agents to arrive and then continue to its original target.

In the MAPF task on map 4, I use the same methods to optimize the computation time in order to demonstrate my solution's generalization capability when the terrain is more complex and there are fewer agents. As shown in Fig. 3.10, 2 regions have more conflicts than others, i.e., the region including vertex (8,6) and vertex (9,6), and the region including vertex (6,10). Same as the method, namely

construction site optimization, used in the previous case, the vertex (5,10) is indicated, which shall be modified from an obstacle to a road with good conditions. In addition, the road condition of the vertex (9,5) and vertex (10,6) shall be changed into good condition from bad condition. After this optimization, the computation time is greatly reduced to roughly 0.17 seconds. For agent optimization, the most conspicuous troublemakers in this task shall be removed, which are agent 10 and agent 11, as shown in Fig. 3.12. Here I removed the agent 10 so that the computation duration reduces to about 0.17 seconds. Although the optimization results of these two methods in the MAPF task on map 4 are almost the same, I recommend layout optimization because only 12 agents were deployed in map 4. If an agent is removed, the overall productivity will be reduced more significantly compared to the previous scenario on map 2. In case that agent 10 did not run perfectly according to the planned schedule and had a delay of one-time step at the beginning, the computation duration for replanning can also exceed 5 seconds, even if the layout of the construction site was optimized with the first method. The algorithm gave a new goal for agent 10, which shall be the vertex (7,4), and the computational time was afterward 0.15 seconds. According to the above results, the algorithm is also proven as effective for the task on map 4.

## **3.7 Advantages of My Methods**

In this study, my approach enables many machines to work simultaneously inside of the working site. Firstly, the method helps civil engineers to arrange the construction site before the site is setup. My approach points out the positions where conflicts occur among the machines and thus indicates the place worthy of being modified. Moreover, my algorithm also helps the engineers to determine the reasonable number of machines in a working site, on the premise of using advanced algorithms. In addition, my algorithm schedules a conflict-free solution for the agents so that the agents can move confidently without hesitation. Last but not least, since the emergencies are inevitable, I design the system to replan the path solution in a very short period compared to the SOTA MAPF algorithm with only slightly increase the non-optimality of the solution.

## **3.8 Conclusion**

In this chapter, I presented an efficient and effective algorithm to calculate the path of a fleet of machines on a construction site. Considering the complicated terrain of a construction site, I endow my algorithm with the ability to handle the weighted maps. By testing my method on five different and diverse maps, my method successfully found the best path for a fleet including participants with different importance. By solving the MAPF problem for a construction site from both algorithmic and construction layout perspectives, I showed the benefits of my hybrid method, especially in reducing the computational time to handle emergencies. Based on my results, modify the unreasonable part is the most efficient fashion to speed up the searching process. Also, removing the agents which cause the most conflicts is always viable and can dramatically reduce the searching time but slightly reduce the whole productivity.

# **4 SLAM for Machines on a Smart Working Site1**

The decision of a reasonable strategy for machines on a working site is not only determined by its intrinsic signals, but also very strongly by environmental information, especially the terrain. Due to the dynamically changing of the construction site and the consequent absence of a High Definition (HD) map, the SLAM offering the terrain information for construction machines is still challenging. Current SLAM technologies proposed for mobile machines are strongly dependent on costly or computationally expensive sensors, such as RTK GPS and stereo cameras, so that commercial use is rare. In this chapter, I proposed an affordable SLAM method to create a multi-layer gird map for the construction site so that the machine can have the environmental information and be optimized and directed accordingly. Concretely, after the machine passes by, I get the local information and record it. Combining with positioning technology, a map of the interesting places of the construction site can be then created. As a result of my research gathered from Gazebo, I showed that a suitable layout is the combination of 1 IMU and 2 differential GPS antennas using the unscented Kalman filter, which keeps the average distance error lower than 2 m and the mapping error lower than 1.3% in the harsh environment. The SLAM technology proposed in the chapter provides the cornerstone to activate the pathfinding solution proposed in the previous chapter.

1 Except some tiny modifications, all the figures, text, and results of the presented work in this chapter have been published in my preprint publication [66]. My contribution to the paper is summarized as 100% in terms of conception and methodology, 90% of literature review, 50% of model building and simulation, 50% of results visualization, and 95% of formulation.

## **4.1 Introduction**

The environment also has an essential influence on the performance of a fleet of working machines, i.e., to perform tasks both efficiently and safely, the construction machines shall be conducted by knowing their location and surroundings; thus, I proposed a method that can generate the map information surrounding the mobile machines only with commodity sensors so that provides the possibility to improve the system further. The basic idea of my approach is to generate the map information of the working site based on the vehicle position, rolling resistance, as well as road grade. Concretely, a special recursive least square with forgetting algorithm is used to record the road grade and the rolling resistance in realtime [84, 41]. These information will be saved together with the localization information. Consequently, after the machine passes by, it will record the information about that place. Since the mobile machines are driving repeatably for a special task, the method can be expected to work well even when the map information does not cover most of the working site. Fig. 4.1 illustrates the motivation of my approach. Safety first

Figure 4.1: Mobile machines perform tasks more efficiently or safer according to their location and surroundings information. The short-term goal of SLAM is to prevent construction machinery from always working in low-efficiency areas for safety reasons, whereas the long-term goal is to increase the productivity of the working site with the help of path planning. The chapter focuses on affordable SLAM technology for construction machines.

### **4.2 Problem Statement**

Although the map information can also be obtained from satellite, it is impossible to get the valuable information, such as a HD map, only depending on remote sensing due to a construction site's fast-changing environment. Also, the sensors can be quite noisy. Especially, they will be further exacerbated on a working site. The sensors, such as Global Positioning System (GPS), Inertial Measurement Unit (IMU), and odometry sensor will have higher measurement errors in case of the harsh environment. In addition, different construction machines have different drivetrain system, which makes a predefined motion model difficult. For instance, since mobile machines may work outside the coverage of base stations, the GPS signal can only achieve nearly 10 m accuracy [85] without signal correction. Last but not least, for passenger cars, the longitude error might not have such a negative effect as the latitude error since further measurements can be adopted to avoid the collision. In the case of construction machines, both errors shall be treated equally.

## **4.3 Goal of This Chapter**

The goals of this chapter are twofold. The first goal of the chapter is to find out the most suitable sensor arrangements for construction machines. For accurate estimation of the position of machines, rather than only trust the measurement from one sensor, I fused a series of different kinds of sensors with the help of sensor fusion technology, derived from Kalman filter [86], to achieve better accuracy. Afterward, the second goal is to create a map with the environment condition by combining the surface resistance, road grade, and position information in realtime. Thanks to this map, further optimization of operation strategy and path planning can be realized. Although I suggest to measure the surface resistance and road grade by recursive least square with multi forgetting factors, the map-building approach I proposed can also be combined with other methods with other kinds of sensors, such as using ultrasonic proposed by Jung [87].

## **4.4 Related Works**

#### **4.4.1 Sensors**

The combination of several sensing systems so that they can compensate the technical shortcoming of each other is well-known in the field of autonomous systems [88, 89, 90]. Therefore, there are a series of researches focusing on sensor fusion. Here I first summary the commonly used sensors for simultaneous localization and mapping (SLAM). Although I agree the introduction of the HD map can surely increase the accuracy of the localization, I do not consider this technology for the construction machines due to the dynamically changing of the construction site, as mentioned in [91].

#### **<IP_ADDRESS> GPS**

Global Navigation Satellite Systems (GNSS) such as GPS, GLONASS, BeiDou, and Galileo rely on at least four satellites to estimate global position at a relatively low cost. Typical standalone GPS average accuracy ranges from few meters to above 20 m [85] due to ionospheric delay, multipath effects, ephemetrics & clock errors, and Geometric Dilution of Precision (GDOP). To improve the accuracy, one of the most used techniques is Differential GPS (DGPS), which utilizes measurements from an onboard vehicle GPS unit and a GPS unit on a fixed infrastructure unit with a known location. Here the known fixed infrastructure unit is called reference station, which calculates the local error in the GPS position measurement periodically. The onboard vehicle GPS units then use this correction to adjust their own GPS estimation. According to [92, 93, 94, 95, 96], an average accuracy in the range of 1–2 m can be achieved, mainly depending on the distance between the vehicle and the base station. Another commonly used improvement is Realtime Kinematic (RTK) GPS, which estimates relative position by means of the phase of the carrier signal and can be expected to achieve centimeterlevel accuracy. Notice that, both of them depend on a fixed base station with a known position nearby, through the principle of them is quite different. In Oct 2020, when I wrote the content of this chapter, RTK GPS is still an extraordinary expensive approach2 and usually be used to define the ground truth position of vehicles . Some low-cost RTK GPS sensors, under 1,000 bucks, are designed with much lower receive frequency [97] and thus cause problems as vehicles driving fast. Thus, in most commercial uses, DGPS is preferable for reducing the cost. Therefore, in my research, I conservatively considered the accuracy of DGPS as 2 m, which is consistent with the normal performance of DGPS. Obviously, as the performance, especially the accuracy, of the GPS increases, my mapping approach will also have better performance consequently.

#### **<IP_ADDRESS> IMU**

Inertial Measurement Units (IMUs) are integrated electronic devices that contain accelerometers, magnetometers, and gyroscopes. It can provide raw IMU measurements to calculate attitude, angular rates, linear velocity, and position relative to a global reference frame.

#### **<IP_ADDRESS> Odometry**

Odometry is the most widely used navigation method for positioning; it provides good short-term accuracy, is inexpensive, and allows very high sampling rates. Odometry is based on simple equations, which hold true when wheel revolutions can be translated accurately into linear displacement relative to the floor. The main advantage of odometry is that all localization information comes from the vehicle itself so that this information is always available. Usually, it is the only localization information when other sensors are not able to provide data. Thus, a good odometry based localization system is always necessary, and it is usually the first step to localization [98].

2 For example, the Trimble R10 costs 18,000 US Dollars on Alibaba.

#### **4.4.2 Localization Technologies**

#### **<IP_ADDRESS> Mobile Robotics**

A series of researches using sensor fusion to achieve a highly accurate localization has been studied worldwide. The technologies about SLAM can be roughly divided into two parts: indoors and outdoors. For indoor localization, such as domestic robots [99], a GPS system cannot be used. However, the road is relatively flat, and thus only a two-dimensional map is needed. In contrast, when it comes to offroad navigation in rough terrain, the algorithms must be capable to handle three dimensions of the environment. After the success of Kalman filter [86], extended Kalman filter [100], and finally unscented Kalman filter [101, 102], the idea that a mobile robot which executes useful missions should be endowed with navigation ability has become a consensus. However, the selection of combining different sensors is from case to case different. For instance, Bento fused the data from ABS sensors and GPS for outdoor localization, based on extended Kalman filter [103], and Zhang integrated the information from GPS and IMU [104]. Also, Li used a camera instead of GPS to accomplish mean positioning errors of 75 cm [105]. In addition, Wolcott proposed a Visual Localization method within LIDAR Maps for Automated Urban Driving [106]. For the cost purpose, Ward studied the possibility to use radar to localize the vehicle's position and demonstrates that errors go to 27.8 cm laterally and 115.1 cm longitudinally by their approach in worst case [107]. To investigate the use of LiDAR for localization, Hata suggests using LiDAR to detect curbs and road markings to create a feature map of the environment and localize vehicles with the help of RTK-GPS and IMU within the map [108]. Another alternative is using ultrasonic sensors, proposed by Jung [87]. Interestingly, as the development of the IoT, more and more researchers are focusing on SLAM by cooperative localization techniques. The basic idea of this approach is to get crucial information even when the perception capability is affected by adverse weather or obstacles from infrastructure or other vehicles. For example, del Peral-Rosado showed the feasibility of 5G based localization technology [109], and Rohani utilized VANET to enhance GPS accuracy to 3.3 m mean level [110].

#### **<IP_ADDRESS> Construction Machines**

For mobile construction machines, the requirements for localization techniques can be different based on different use cases. Some machines may work in the underground, where the situation is similar to working in a tunnel, whereas others might work on an open-pit mining site. In underground mines, it was proposed to use the laser for extracting the wall positions, and dynamically generate a path from these laser data while considering variable offsets [111, 112]. In contrast, for the open-pit site, an autonomous wheel loader introduced by Gu [113] uses a set of sensors, including GPS and IMU for localization, and LIDAR, radar as well as a camera for obstacles capture and identification, ensuring it perceives surroundings accurately. Moreover, Xiang created a dataset for mobile machines detection from the view of a camera fixed on the ground [64], while Bang proposed a method recognizing the machines from a view of the drone [114]. In additon, the visual SLAM is proposed [115]. Besides that, V2X technology was introduced in the field of construction machines [67, 68]. In 2020, Xiang proposed to use WiFi to achieve the communication between different vehicles by introducing a realtime estimation method with respect to package loss and delay [116]. Afterward, the feasibility of using 5G for machines is also investigated by [62]. To avoid additional costs for the vehicles, smartphones show great potential to be utilized as a solution to complement the flaws of onboard ECU [117, 40].

In summary, similar to general autonomous vehicles, most automated mobile machines fuse information from onboard sensors such as IMU and GPS by using diverse sensor fusion technologies. Furthermore, camera, LIDAR, and radar are used to detect the environment on the construction site, to avoid obstacles, and to instruct the machines where to go. However, owing to the harsh environment and diversity of working sites, LIDAR and radar can be sensitive. In the recent future, wireless communication can also contribute to better localization of vehicles.

## **4.5 Model Building**

The wheel loader used in the simulation was modeled in Solidworks and then imported into the Robot Operating System (ROS) to explore the approaches that should be used for mobile machines. Since the first goal of this chapter is to find out suitable sensor arrangements to accurately localize wheel loaders, I fused different arrangements of sensor data. Concretely, I used up to three IMUs and three GPSs in the simulation. Based on the characteristic of GPSs, three GPSs were fixed on the cab of the wheel loader. I then installed two IMU sensors under the front axle and other IMUs under the rear axle, based on the suggestion from Li [118]. Fig. 4.2 illustrates the wheel loader model I used in the Gazebo environment.

Figure 4.2: Wheel loader model in Gazebo: once the models had been developed in Solidwork, they were converted to Unified Robotic Description Format (URDF), using a 3rd party URDF conversion tool called "sw\_urdf\_exporter", which allows for conveniently export SW Parts and Assemblies into a URDF file. Gazebo enables us to obtain sensors' simulation such as IMUs, GPSs, encoders, cameras, and stereo cameras through gazebo\_plugins, which can be used to attach into ROS messages and service calling the sensor outputs, i.e., the gazebo\_plugins create a complete interface (Topic) between ROS and Gazebo.

As we know, the URDF is an XML file format used in ROS to describe all elements of a vehicle. URDF can specify the kinematic and dynamic properties of a single robot in isolation. To make my vehicle works properly in Gazebo, additional simulation-specific tags concerning the vehicle pose, frictions, inertial elements, and other properties have been added. The transform tree is shown in Fig. 4.3.

Each Link in URDF represents a rigid body. Also, according to the kinematic and dynamic model shown in Fig. 4.3, the wheel loader in the simulation is divided into several parts, including,

In this project, the wheel loader receives GPS data from an onboard GPS sensor plugin with its latitude and longitude; however, the GPS data provided by the GPS plugin cannot be directly applied to the fusion of the sensor data, so coordinate system conversion for GPS data is required. For the simulation, I set a transform for each GPS that converts the vehicle's world frame coordinates, i.e., the frame with its origin at the vehicle's initial position, to the GPS's UTM coordinates, the same as [119], as

Figure 4.3: The dynamic system simulated by URDF file on ROS.

$$\mathbf{T} = \begin{bmatrix} c\theta c\psi & c\psi s\phi s\theta - c\phi s\psi & c\phi c\psi s\theta + s\theta s\psi & x\_{UTM\_0} \\ c\theta s\psi & c\phi c\psi + s\phi s\theta s\psi & -c\psi s\phi + c\phi s\theta s\psi & y\_{UTM\_0} \\ -s\theta & c\theta s\phi & c\phi s\theta & z\_{UTM\_0} \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{4.1}$$

where φ, θ, ψ denote the vehicle's initial UTM-frame roll, pitch, and yaw. c and s denote the cosine and sine functions, respectively. xUTM0 , yUTM0 , and zUTM0 are the UTM coordinates of the first reported GPS position. After that, the GPS signal is then transformed into the vehicle's world coordinate frame, odom, by

$$\begin{bmatrix} x\_{odom} \\ y\_{odom} \\ z\_{odom} \\ 1 \end{bmatrix} = \mathbf{T}^{-1} \begin{bmatrix} x\_{UTM\_t} \\ y\_{UTM\_t} \\ z\_{UTM\_t} \\ 1 \end{bmatrix} \tag{4.2}$$

In my simulation the ROS package "robot\_localization" from Moore [119] was used, including a "navsat\_transform" node, which provides functions to convert between various coordinate frames and integrate GPS data. It provided a transformation function that allows the conversion between GPS frame, expressed in latitude and longitude, and vehicular coordinate. This process shall be carried out for each GPS independently.

In practical applications, GPS signals can be received infrequently. Yet the localization technology must maintain state estimation even when some of the vehicles' signals are absent. Therefore, the performance of the filters when GPS signals infrequently arrive in the system shall be evaluated. Taking this problem into consideration, I used a ROS node built by Li [118] to filter the collected GPS signals such that GPS data is unavailable for 1 second once every 10 seconds. In case multi GPS sensors are used, the signal failure might not happen at the same time. I aim to observe how the filter and my approach behave with different sensor configurations when some GPS signals go wrong.

### **4.5.1 Sensor Fusion for Localization**

For localization of the wheel loader in the simulation environment, I used EKF and UKF node in "robot\_localization" [119]. On the one hand, this package has no limitation for the number of sensor inputs, which just in line with my construction machines' requirements. On the other hand, a concrete motion model is not needed so that this method can easily be used on both excavators and wheel loaders with different drivetrain solutions. In "robot\_localization", the filter's state will be driven forward by a standard 3D kinematic model derived from Newtonian mechanics to calculate the vehicle's motion, including position, velocity, and acceleration in three dimensions.

In the correction step, the measurement model integrates sensor data to update the predicted state. GPS provides position information, and wheel encoders provide velocity information for the correction. Moreover, the orientation, velocity, and acceleration information are updated from IMUs via gyroscopes and accelerometers. Furthermore, the process model and measurement model also need to add a noise covariance matrix, Q and R, respectively. The noise matrices can generate uncertainty in the system. The process matrix contributes to the overall uncertainty in the algorithm, which adds to the process model. Intuitively, a large value in the Q matrix means a considerable uncertainty in the process model, causing the system to have greater confidence in the measurement data. In the current implementation, the process noise matrix was set as a diagonal matrix. The state variables, which are directly measured by the sensors, such as x y position by GPS and orientation by IMUs, were set relatively small. The variables, which were not directly measured, could be updated from the measured data. The measurement covariance matrix R corresponding to the confidence in the sensor data. Similarly, the greater the noise in the elements of this matrix, the less confidence in the measurement data. The measurement covariances are derived by the sensors noisy.

In addition to the inaccuracy of filters, outliers are also an important source of error. In my simulation, I assume that the measurements have Gaussian distributions. Although sensors follow the normal distribution as setting, improbable, and extremely noisy measurements can appear due to the high fidelity of ROS. To counteract this problem, I used Mahalanobis distance to detect outliers and thus overcome the consequent adverse effect. After this, the filtered data were used for state correction. Concretely, the Mahalanobis distance is calculated as a product of the from filter processed vector to find out the outliers,

$$D\_M(\vec{x}) = \sqrt{(Z\_t - \hat{Z}\_t)^T A^{-1} (Z\_t - \hat{Z}\_t)} \tag{4.3}$$

where A is the covariance matrix.

#### **4.5.2 Sensor Fusion Methods**

State estimation is one of the most critical issues in many autonomous applications. Having an accurate state estimation, the machines can be effectively navigated in the environment and thereby making optimal decisions for specific purposes. For instance, to reach a target destination, it needs to know its current state, which consists of position, velocity, acceleration, and heading to execute the right maneuvers correctly. Since sensors are susceptible to noise and imperfections introducing uncertainty to the measurements, the filter's goal is to fuse all the available sensor data, as well as the vehicle's own dynamics to obtain a more precise estimation of the vehicle's state. As mentioned, two necessary extensions of the Kalman filter are presented, notably the EKF and UKF.

The filters are modeled to improve the positioning's accuracy by compensating for the disadvantages of the different sensors. As we know, GPS provides relatively accurate positioning, but the signal's availability remains a problem, especially in urban and mountainous environments. This determines that the results of using GPS alone are usually not satisfactory. Also, IMUs use a combination of accelerometers and gyroscopes to measure linear accelerations and angular velocities, respectively. By estimating the position relative to its initial position, the trajectory can be calculated by these information of the wheel loader as the vehicle travels. For sure, there is also a common problem with IMUs, namely the accumulated errors. To avoid accumulated drift and provide global positioning, the estimated position shall be corrected by using other sensors, e.g. GPS. Without a doubt, a GPS/IMU system is successful in increasing the accuracy beyond standalone GPS or IMU capabilities.

#### **<IP_ADDRESS> Extended Kalman Filter**

Although linear Gaussian systems are abundant, most systems, in reality, are non-linear. Also, they often do have Gaussian noise. Wrong assumptions about the system can lead the Kalman filter to diverge and provide estimation with very high errors. Consequently, multiple extensions have been developed to deal with various scenarios encountered in practice. One of the famous variations is the EKF, where it deals with non-linearity by approximating a linear equivalent before performing the required filtering sequence. The idea of the EKF is that if the system is close to linear for short periods, using its linear approximation will then not yield large errors.

Through linearizing the basic equations from Welch [102], the following equations are obtained:

$$x\_{k+1} \approx \tilde{x}\_{k+1} + B(x\_k - \hat{x}\_k) + Ww\_k \tag{4.4}$$

$$z\_k \approx \tilde{z}\_k + H(x\_k - \hat{x}\_k) + Vv\_k \tag{4.5}$$

where xk+1 and zk are the actual state and measurement vectors, x˜k+1 and z˜k are the approximate state and measurement vectors. xˆk is an a posteriori estimate of the state at step k, random variables wk and vk represent the process and measurement noise. B is the Jacobian matrix of partial derivatives of f(•) with respect to x, W is the Jacobian matrix of partial derivatives of f(•) with respect to w, H is the Jacobian matrix of partial derivatives of h(•) with respect to x, V is the Jacobian matrix of partial derivatives of h(•) with respect to v.

An essential feature of the EKF is that the Jacobian Hk in the equation for the Kalman gain Kk serves to correctly propagate only the relevant component of the measurement information [102]; the linearization error is always exist as the function is nonlinear. Because increasing the sampling time and reducing the nonlinearity of function are not always viable, error-state EKF is proposed to counteract the adverse effect. The basic idea of error-state EKF is to reduce the distance of the linear approximation from the operating point; instead of linearization of the nominal state, it handles the error state.

#### **<IP_ADDRESS> Unscented Kalman Filter**

When the state transition and observation models, that is, the predict and update functions f and h are highly nonlinear, the EKF can give particularly poor performance. This is because the covariance is propagated through the linearization of the underlying nonlinear model. By contrast, the UKF uses unscented transform instead of linearization in the prediction and correction steps to make the estimation. As the first step, the decomposition of the covariance matrix shall be computed and the sample points shall be carefully selected, for instance, here I selected 2L+1 points, described as,

$$\begin{aligned} \mathcal{X}^0 &= \bar{x} \\ \mathcal{X}^i &= \bar{x} + (\sqrt{(L+\lambda)P\_x})\_i \\ \mathcal{X}^{i+L} &= \bar{x} - (\sqrt{(L+\lambda)P\_x})\_i \end{aligned} \qquad \begin{aligned} i &= 1, \dots, L \\ i &= 1, \dots, L \end{aligned} \tag{4.6}$$

where x¯ is the selected mean sample point and λ is set as λ = 3− N for Gaussian probability density function. After that, the sigma points will be propagated through the nonlinear function as,

$$\check{X}^i = f(\mathcal{X}^i) \qquad i = 1, \dots, 2L \tag{4.7}$$

where f(•) is the motion model function and and Xˇi is the predicted position based on the motion model. As the final step of predicted process, the predicted mean and covariance should be calculated.

$$a^i = \begin{cases} \lambda/L + \lambda, i = 0\\ \lambda/2(L + \lambda), otherwise \end{cases} \tag{4.8}$$

$$
\check{X} = \sum\_{i=0}^{2N} a^i \check{X}^i \tag{4.9}
$$

$$\check{P} = \sum\_{i=0}^{2N} a^i (\check{X}^i - \check{X})(\check{X}^i - \check{X})^T + Q \tag{4.10}$$

Here Q denote the noisy of this process. In the correction step, I firstly calculate the predict measurement with the sigma points,

$$\hat{y}^{(i)} = h(\check{X}^i, 0), i = 0, \dots, 2N \tag{4.11}$$

where h(•) is the predicted measurement model, considering the process noise: LˇLˇT = Pˇ. Then, I get the mean and covariance of predicted measurements,

$$\mathcal{Y} = \sum\_{i=0}^{2N} a^i \mathcal{Y}^i \tag{4.12}$$

$$\check{P}\_y = \sum\_{i=0}^{2N} a^i (\mathcal{Y}^i - \mathcal{Y}) (\mathcal{Y}^i - \mathcal{Y})^T + R \tag{4.13}$$

where R is the noisy. Based on the previous result, I can compute the crosscovariance and Kalman gain, and then get the corrected covariance and mean.

$$P\_{xy} = \sum\_{i=0}^{2N} a^i (\check{X}^i - \check{X})(\mathcal{Y}^i - \mathcal{Y})^T \tag{4.14}$$

$$K = P\_{xy} P\_y^{-1} \tag{4.15}$$

$$
\hat{P} = \check{P} - KP\_yK^T \tag{4.16}
$$

$$\mathcal{X}^i = \check{X} - K(y^0 - \mathcal{Y}^i) \tag{4.17}$$

where y 0 is the measurement result with respect to Xˇ, X i is the final results from UKF. As can be seen, UKF does not need Jocobian matrix so that it can achieve a better performance in estimation. Usually, the computation effort can be slightly higher than the other methods, making carefully select sensors configurations meaningful.

#### **4.5.3 Realtime Map Plotter**

Inspired by the research from Fankhauser [82], a map can include many layers to store different types of data information. Thus, to develop a realtime plotter of the construction site according to ground condition, I use a multi-layer grid-based map, which divides the environment into uniform cells. Fig. 4.4 illustrates the multilayered grid map concept, where each cell data is stored on the congruent layers. In this project, since resistance and grade of the road are the most of importance information for the construction machines, I adopt a two-layer grid map. However, I show the method to save three different information in this chapter. Concretely in this chapter, the map is divided into small cells, whose resolution is 1 m per cell. Notice that I use a much smaller cell than the cell used in the previous chapter. This is on one hand because the measurement did by foot SLAM cannot cover so large areas in one step. On the other hand, a finer grid map describes the terrain information better. Apparently, the map used in the previous chapter can be processed from the finer map providing in this chapter. As I discussed in the previous section, I use GPS/IMU fusion Kalman filter algorithms to locate the mobile machine on the construction site.

As can be seen in Fig. 3.4, to describe the ground condition of construction sites, I use the value of each cell to represent the information of the ground situation. An

Figure 4.4: A grid map created by SLAM. My approach uses multilayered grid maps to store data for different types of information. Concretely, every grid saves a 1\*3 matrix including location information and resistance or grade, depending on which layer it is. A grid with site information will be created after the vehicle passes by. The map is saved as a 2\*m\*n\*3 tensor, where m is the max displacement in the x-direction, while n is the max displacement in the y-direction. In case a grid does not be occupied once by the vehicle, it will be marked as NaN to denote the unknown regions.

exemplar layer that holds the data of a grid-based map has been shown in Fig. 3.4 in the previous chapter. Obviously, although I only demonstrate the approach with two layers, it is relatively easy to extend the third layer in case that uphill or downhill is vital, as the third layer is responsible for recording the heading of the vehicle.

Based on the previous study [84], both resistance and grade of the road can be gathered in realtime. Thus, I assume that the ground resistance and slope are known after the mobile machine passed by. When the mobile machine passes through each grid cell, ground information will be added to the corresponding grid of the two layer-grid-maps. Concretely, the plotting algorithm combines the localization results from the Kalman filter and the grade and resistance information from the recursive least square algorithm. To implement the plotting algorithm in ROS, a node was created in ROS that can subscribe to the localization results from the Kalman filter node and gather the current information from ground truth maps with the assumption that resistance and grade of the road can be estimated or measured well [118].

## **4.6 Vehicle Simulation Scenarios in ROS and Gazebo**

To test the feasibility of the map plotter, different road conditions on a construction site was first defined. ROS is a mature and flexible framework for robotics programming, providing the required tools to easily access sensors data, process that data, and generate appropriate responses for the robot's actuators. Due to these characteristics, ROS is a perfect tool for many types of research on modern robotics. After all, a mobile machine can be considered as just another type of robotics, so the same types of programs can be used to develop advanced construction machines. In this chapter, a construction site showed in Fig. 4.5 was simulated. According to this real construction site, five different ground resistances in the simulation environment are defined, according to different ground conditions. Moreover, two areas according to different slopes. Concretely, the different regions were distinguished based on road material.

5. Dry concrete surface: Dry concrete is a normal building material and the typical rolling resistance coefficient of the dry concrete road is 0.008.

Besides, two different ground slopes were also defined appropriately. Since only a two-layer grid map is used in this research, I do not discriminate uphill or downhill.

For simulation, the green area was defined as the dry concrete surface in the ground-truth resistance map. The red area represents the gravel surface. The blue area represents the sand surface, the black area, and the yellow area represent dry and wet dirt roads, respectively. With this premise, the ground truth map upon this construction site is drawn. Same as in the ground-truth resistances map, the ground slope map is also defined, where the green area represents the flat area and the red area represents the 15° slope.

In this project, the plot\_node [118] was written with Python and OpenCV library to visualize the plotted map. To test the feasibility of a realtime plotter in simulation, a ground\_truth node [118] is used in ROS, which provides a ground truth position of the simulated mobile machine in Gazebo. When the mobile machine moves to a certain position, the system uses the ground truth position to determine the rolling resistance coefficient and the road grade, and then uses the Kalman filter filtered position to plot the corresponding information in the grid-based map.

In ROS and Gazebo, the build-in plugins provide many adjustable parameters that can be used to adjust the devices' performance. To get closer to reality, the performance parameters of GPSs and IMUs was set in the simulation according to real GPS and IMU devices. To get the best sensor configuration, different sensor

(a) Example construction site divided into five areas according to different ground resistances.

(b) Example construction site divided into two areas according to different slopes.

Figure 4.5: The ground truth map with dimensions. The simulation environment I used in Gazebo was modeled based on a real construction site, and the parameters are selected according to material characteristics. Since simulating a small construction site may cause system error and thus lack plausibility, I augmented this real construction site's dimensions in Gazebo.

configurations for different algorithms were implemented. Each group of sensor

configurations were simulated under the same condition by rosbag, and the results were output simultaneously.

## **4.7 Experiment and Results**

#### **4.7.1 Localization Results**

To explore the most suitable sensor arrangements of the Kalman filter for construction machinery, the results from different sensor configurations with different methods were compared. The concrete sensor arrangements in this project are shown in Tab. 4.1.

Table 4.1: Sensor configuration. In my simulation, I ignore the mirror difference caused by the slightly different installation position of sensors. Therefore, the various configurations are reduced from 64 to 16. Since odometry is robust and necessary for many applications, I do not consider the case without an encoder

To evaluate the performance of the different variants of sensors and algorithms concerning accurately positioning, I controlled the wheel loader to drive on the previously defined construction site in Gazebo, and recorded the data from sensors at the same time. Afterward, the localization results of the different methods are compared to the ground truth. Here I use the Root-Mean-Square Error (RMSE) as a quantitative indicator of the error to assess the pose estimation results.

$$RMSE = \sqrt{\frac{\sum\_{i=1}^{n} \left(\tilde{S}\_i - \hat{\tilde{S}}\_i\right)^2}{n}} \tag{4.18}$$

Where S ˆ˜ is the vector including estimated x and y position, S˜ is the denotes ground truth x and y position, n is number of all estimated samples, and the footnote i denotes the i th time step.

Fig. 4.6 shows the wheel loader estimated trajectories given by different approaches and the ground truth trajectory, where the red lines are the ground truth trajectories that the vehicle passes, and the blue lines are the estimated position of the vehicle. Since only EKF and UKF are capable of handling the nonlinear problem, I compare the results obtained using EKF filtering and the UKF filtering technique with different sensor arrangements. Notice that, odometry sensor is always used though I do not explicitly mention it. Apparently, the UKF performs better than the EKF, which is also in line with the conclusion from most studies. Generally speaking, with GPS fusing in the estimation, the accuracy improves drastically. Also, since the GPS may lose signal every 10 seconds, an additional GPS sensor can surely increase the position accuracy. In contrast, more IMUs can only slightly improve the accuracy of positioning. As can be seen, the IMU + Odometry estimation method yields inferior performance no matter with EKF or UKF. Once the IMU reports an inaccurate heading, the errors will be accumulated, causing the measured position drifts further away from its true position as the wheel loader travels.

Fig. 4.7 shows a comparison of localization error between the different approaches with respect to time. As shown in Fig. 4.7, the RMSE and the euclidean distance

Figure 4.6: The IMU + Odometry estimation method yields for both EKF and UKF the RMSE over 70 m. Except for the IMU + Odometry methods, the other results can be divided into four performance levels according to the error scale. The worst level is EKF with 1 GPS, where the RMSE is about 3 ∼ 4 m. The second level is EKF with 2 GPSes and EKF with 3 IMUs and 3 GPSes. In these cases, the RMSEs are about 2 ∼ 3 m. A better level is UKF with 1 GPS and 1 IMU, where the RMSE of the third level can be achieved about 2 ∼ 2.5 m. Finally, my experiment's best class is UKF with 2 GPSes and 1 IMU, and UKF with 3 IMUs and 3 GPSes, which reduce the RMSE to about 1.2 m.

error of each sensor arrangement can be obtained, indicating that the EKF has more significant tracking error than UKF. It happens because the linearization through its Jacobian is an approximation, and the kinematic model of the wheel

Figure 4.7: Quantitative evaluation of different methods. Here (a) and (b) show the accumulative error, i.e., RMSE, while (c) and (d) demonstrate the current Euclidean distance error of each sensor arrangement. There are some noticeable instantaneous position changes every ten seconds due to infrequent GPS signal loss. Generally, UKF shows better performance than EKF for both RMSE and euclidean distance error.

loader is highly nonlinear. To get a more intuitive and accurate description of the error for each sensor configuration and method, Tab. 4.2 is created to demonstrate the RMSE in detail.

As aforementioned, the GPS signal will be lost for one second every ten seconds. Thus, there are some noticeable instantaneous position changes every ten seconds. The loss of GPS signals clearly causes these jumps. As can be seen, for sensor configuration with only one GPS, the leap of the localization error as well as the variance values are more sharply. Obviously, with the number of GPS increases, the jumps are diminished and therefore become more acceptable.

Table 4.2: Localization errors of each approach

Apparently, the simulation results show that UKF is a better approach using data collected by onboard sensors of the wheel loader in gazebo environments. Intuitively, more sensors represent higher accuracy. However, the results show that an appropriate number of sensors can achieve acceptable accuracy at a lower cost. As can be seen in Tab. 4.2, the RMSE of UKF with 1 IMU 2 GPSes is 1.72 m, and UKF with 3 IMUs and 2 GPSes is 1.40 m. With additional 2 IMUs and 1 GPS, the RMSE is only slightly reduced by 0.3 m. More importantly, the maximal error is strongly diminished by one additional GPS, whereas continually increasing the sensor number does not further reduce the error proportionally. Of course, according to different application scenarios, different sensor configurations shall be chosen. For my application scenario, UKF with 1 IMU and 2 GPSes has sufficient accuracy and a better economy respecting sensor hardware cost and onboard ECU computational effort. Thus, I suggest using this sensor configuration to locate the mobile machines and then develop the realtime map plotter.

### **4.7.2 Plotter results**

As my ultimate goal is to create a map of the current working site in realtime based on localization technology so that corresponding optimization can then be achieved, the ground truth maps and estimated maps with different sensor configurations coupled with various algorithms are shown and compared in Fig. 4.8.

Figure 4.8: The ground truth and estimated maps. In the ground resistance map and road grade map plotted by EKF with 1 IMU and 1 GPS, the spikes caused by infrequent GPS are quite obvious. With additional GPS sensor fused in Kalman filter, the spikes improve a lot.

Since I use a two-layer grid map, both rolling friction coefficient and road grade are recorded and used to create the estimated maps. In this chapter, the ground information are saved in corresponding grid after the wheel loader passes by and identify the ground information such as friction and slope through the specific algorithm, e.g. recursive least square. As aforementioned, to locate the mobile machine's position in a cost-efficient fashion, the configuration of the UKF with 1 IMU and 2 GPSes is adopted. Also, in order to compare the performance of the selected configuration, I draw the results of the EKF with 1 IMU and 1 GPS and the UKF with 1 IMU and 1 GPS as the control groups. The mispredicted points are calculated as described in Eq. 4.19,

$$E\_{r,s} = \sum\_{i=1, j=1}^{m,n} e\_{i,j} = \begin{cases} 1, & \text{if } \neg(\mathring{G}\_{i,j} = G\_{i,j}) \cap \neg(\mathring{G}\_{i,j} = NaN) \\ 0, & \text{if } (\mathring{G}\_{i,j} = G\_{i,j}) \cap \neg(\mathring{G}\_{i,j} = NaN) \\ 0, & \text{if } \mathring{G}\_{i,j} = NaN \end{cases} \tag{4.19}$$

where Gˆ i,j denotes the estimated grid map information, Gi,j is the ground truth map information, E is the accumulated number of errors, the subscript r and s denotes resistance and slope map, respectively. Obviously, the goal is to minimize the percent of mispredicted points versus total predicted points, described as Eq. 4.20,

$$\min J\_{r,s} = \frac{E\_{r,s}}{T} \tag{4.20}$$

where T is the total estimated number, and J is the quantitative criteria to evaluate the accuracy of the plotted map.

The localization errors of each group are shown in Tab. 4.3, where the necessity of the introduction of the second GPS sensor, and the adoption of unscented Kalman filter is shown.

Table 4.3: Localization errors of each approach during the plotting

To evaluate the plotted maps' accuracy, Matlab was used to implement an algorithm [118] to compare the predefined area and the plotted path the mobile machine traveled. Fig. 4.9 graphically illustrate the mispredicted grid point with white color, where the mispredicted points are generally distributed in the marginal zone. This is because even the localization technology makes some mistakes; the problem is unlikely to cause an error as long as the vehicle is not at the very edge of different zones, indicating the robustness of this mapping idea of the construction site.

(a) Result of EKF with 1 IMU and 1 GPS. (b) Result of UKF with 1 IMU and 1 GPS. (c) Result of UKF with 1 IMU and 2 GPSes.

(d) Result of EKF with 1 IMU and 1 GPS. (e) Result of UKF with 1 IMU and 1 GPS. (f) Result of UKF with 1 IMU and 2 GPSes.