This compilation thesis presents two main contributions in path planning and obstacle avoidance, as well as an integration of the proposed modules with other frameworks to enable resilient robotic missions in complex environments.In general, through different types of robotic missions it is important to have a collision tolerant and reliable system, both regarding potential risks from collisions with dynamic and static obstacles, but also to secure the overall mission success.%Particularly, a common trend in the presented work is safety regarding collisions with dynamic and static obstacles, as well as reliable overall systems that are capable of executing missions.

The work included in this thesis presents the risk-aware path planner D$^*_+$ that is capable of planning traversable paths for both ground and aerial robots. D$^*_+$ is developed on top of D$^*$-lite with a risk layer close to occupied space, modeling the unknown areas as a risk, and is implemented with a dynamic map to enable updates and adjustments to a changing environment.

The risk layer aids in solving two common challenges with path planning for real robots: a) it creates a safety margin that gives free space between the path and obstacles so that robots with the corresponding size can follow the path, and b) it masks smaller holes in walls that occur when building maps from real data.

Using a dynamic map makes it possible to use D$^*_+$ for an exploration mission, it also enables for the re-planning of the path if the environment changes for example, if an obstacle suddenly blocks a path, a new path will be planned. D$^*_+$ have been tested in different real-life experiments with both an Unmanned Areal Vehicle (UAV) and a quadruped-legged robot and shown to produce traversable paths in different application scenarios, such as exploration, return to base, and navigation on known maps.

This thesis also presents an obstacle avoidance architecture for velocity objects, structured around an object detection and tracking scheme that is combined with non-linear model predictive controller (NMPC) to plan the avoidance maneuver. %that uses a Convolutional Neural Network to detect obstacles that are tracked so they can be avoided by a non-linear model predictive controller (NMPC).In this case, the detection is done with the Convoluitonal Neural Network (CNN) You Only Lock Once v4 (YOLO) where the most certain human is tracked with a Kalman filter, and the velocity of the human is estimated.The proposed scheme models the object motion as constant velocity, which is utilized from the NMPC to plan control inputs for the robot to avoid the identified obstacle. A merit of the approach is that the avoidance maneuver does not only consider the current identification position, but also considers the motion prediction of the object. This avoidance framework proved to be capable to avoid non-cooperative obstacles, such as humans moving towards it.Due to the fact that the avoidance is starting when a future collision is predicted, the avoidance maneuver is started early enough to avoid obstacles with a higher velocity than a classic ``static obstacle'' radius approach can handle.

An additional aim of this thesis is to showcase that the proposed contributions can be applied in full robotic missions/frameworks. Thus, this thesis presents a search and rescue mission with a quadruped-legged robot and a UAV on a partially known map to find the location of survivors and other objects of related interest. In this mission, the quadruped-legged robot carries the UAV to the edge of the known map from where it launches the UAV that then explores and detects any survival and other relevant objects.Also, an autonomy solution, based on Boston dynamics' quadruped-legged robot Spot, for enabling a map-based navigation in confined environments has been developed and tested. This Spot solution enables the robot to navigate to a user-selected point, rotate in the desired direction, and instruct the UAV, in the combined search and rescue mission, to take off.

Robots are increasingly utilized in industry for their capability to perform repetitive,complex tasks in environments unsuitable for humans. This surge in robotic applicationshas spurred research into Multi-Robot Systems (MRS), which aim to tackle complex tasksrequiring collaboration among multiple robots, thereby boosting overall efficiency. However,MRS introduces multifaceted challenges that span various domains, including robot perception,localization, task assignment, communication, and control. This dissertation delves into theintricate aspects of task assignment and path planning within MRS.The first area of focus is on multi-robot navigation, specifically addressing the limitationsinherent in current Multi-Agent Path Finding (MAPF) models. Traditional MAPF solutionstend to oversimplify, treating robots as holonomic units on grid maps. While this approachis impractical in real-world settings where robots have distinct geometries and kinematicrestrictions, it is important to note that even in its simplified form, MAPF is categorized as anNP-hard problem. The complexity inherent in MAPF becomes even more pronounced whenextending these models to non-holonomic robots, underscoring the significant computationalchallenges involved. To address these challenges, this thesis introduces a novel MAPF solverdesigned for non-holonomic, heterogeneous robots. This solver integrates the hybrid A*algorithm, accommodating kinematic constraints, with a conflict-based search (CBS) for efficientconflict resolution. A depth-first search approach in the conflict tree is utilized to accelerate theidentification of viable solutions.The second research direction explores synergizing task assignment with path-finding inMRS. While there is substantial research in both decentralized and centralized task assignmentstrategies, integrating these with path-finding remains underexplored. This dissertation evaluatesdecoupled methods for sequentially resolving task assignment and MAPF challenges. Oneproposed method combines the Hungarian algorithm and a Traveling Salesman Problem (TSP)solver for swift, albeit suboptimal, task allocation. Subsequently, robot paths are generatedindependently, under the assumption of collision-free navigation. During actual navigation, aNonlinear Model Predictive Controller (NMPC) is deployed for dynamic collision avoidance. Analternative approach seeks optimal solutions by conceptualizing task assignment as a MultipleTraveling Salesman Problem (MTSP), solved using a simulated annealing algorithm. In tandem,CBS is iteratively applied to minimize the cumulative path costs of the robots.

This PhD thesis advances robotic autonomy by developing novel path-planning and collision-avoidance solutions that enable resilient missions in complex, unstructured real-world environments. The primary contribution is D*+, a risk-aware path planner extending the D*-lite framework for ground and aerial robots. D*+ introduces a risk layer around occupied and unknown spaces, ensuring traversable paths with safety margins while operating on imperfect maps from real data. Its dynamic mapping supports adaptive replanning, enabling exploration missions in unknown environments, and is excellent for waypoint navigation. Real-world trials with a UAV and quadrupedal robot confirm its versatility across diverse scenarios.

The second contribution is the Detect Track and Avoid Architecture (DTAA), which tackles dynamic obstacles using YOLO-based detection, Kalman filter state estimation, and a nonlinear model predictive controller (NMPC) for anticipatory avoidance maneuvers. DTAA effectively handles fast-moving objects while following D*+ paths; however, it is limited by a short predictive horizon and susceptible to local minima. To overcome these weaknesses, this thesis introduces A*+T, a distributed, time-dependent multi-agent path planner. Built on an A*framework, A*+T integrates D*+ 's risk layers and DTAA's dynamic obstacle handling, adding a temporal dimension to the planning process, enabling collision checks in time and space. The temporal dimension enables distributed autonomous robots to plan collision-free paths in shared spaces based on other robots' planned paths.

Leveraging shared paths and predicted paths from DTAA, A*+T plans collision-free paths around the dynamic obstacles. Validated through simulations and real-world experiments, A*+T enhances mission readiness for multi-agent scenarios.

Beyond these, the thesis integrates these modules into complete robotic systems, enhancing mission control for large-scale applications. Demonstrations include mining inspections (visual and gas detection) and search-and-rescue missions (locating humans/objects). These original advancements offer robust, practical solutions for robotic navigation, validated through extensive real-world testing, and contribute significantly to autonomous systems in high-stakes environments.