Planning robot formations with fast marching square including uncertainty conditions

This paper presents a novel algorithm to solve the robot formation path planning problem working under uncertainty conditions such as errors the in robot's positions, errors when sensing obstacles or walls, etc. The proposed approach provides a solution based on a leader-followers architecture (real or virtual leaders) with a prescribed formation geometry that adapts dynamically to the environment. The algorithm described herein is able to provide safe, collision-free paths, avoiding obstacles and deforming the geometry of the formation when required by environmental conditions (e.g. narrow passages). To obtain a better approach to the problem of robot formation path planning the algorithm proposed includes uncertainties in obstacles' and robots' positions. The algorithm applies the Fast Marching Square (FM2) method to the path planning of mobile robot formations, which has been proved to work quickly and efficiently. The FM2 method is a path planning method with no local minima that provides smooth and safe trajectories to the robots creating a time function based on the properties of the propagation of the electromagnetic waves and depending on the environment conditions. This method allows to easily include the uncertainty reducing the computational cost significantly. The results presented here show that the proposed algorithm allows the formation to react to both static and dynamic obstacles with an easily changeable behavior.This work is included in the project number DPI2010-17772 funded by the Spanish Ministry of Science and Innovation and has been supported by the CAM Project S2009/DPI-1559/ROBOCITY2030 II, developed by the research team RoboticsLab at the University Carlos III of Madrid.Publicad

Planning and estimation algorithms for human-like grasping

Mención Internacional en el título de doctorThe use of robots in human-like environments requires them to be able to sense and model unstructured scenarios. Thus, their success will depend on their versatility for interacting with the surroundings. This interaction often includes manipulation of objects for accomplishing common daily tasks. Therefore, robots need to sense, understand, plan and perform; and this has to be a continuous loop. This thesis presents a framework which covers most of the phases encountered in a common manipulation pipeline. First, it is shown how to use the Fast Marching Squared algorithm and a leader-followers strategy to control a formation of robots, simplifying a high dimensional path-planning problem. This approach is evaluated with simulations in complex environments in which the formation control technique is applied. Results are evaluated in terms of distance to obstacles (safety) and the needed deformation. Then, a framework to perform the grasping action is presented. The necessary techniques for environment modelling and grasp synthesis and path planning and control are presented. For the motion planning part, the formation concept from the previous chapter is recycled. This technique is applied to the planning and control of the movement of a complex hand-arm system. Tests using robot Manfred show the possibilities of the framework when performing in real scenarios. Finally, under the assumption that the grasping actions may not always result as it was previously planned, a Bayesian-based state-estimation process is introduced to estimate the final in-hand object pose after a grasping action is done, based on the measurements of proprioceptive and tactile sensors. This approach is evaluated in real experiments with Reex Takktile hand. Results show good performance in general terms, while suggest the need of a vision system for a more precise outcome.La investigación en robótica avanza con la intención de evolucionar hacia el uso de los robots en entornos humanos. A día de hoy, su uso está prácticamente limitado a las fábricas, donde trabajan en entornos controlados realizando tareas repetitivas. Sin embargo, estos robots son incapaces de reaccionar antes los más mínimos cambios en el entorno o en la tarea a realizar. En el grupo de investigación del Roboticslab se ha construido un manipulador móvil, llamado Manfred, en el transcurso de los últimos 15 años. Su objetivo es conseguir realizar tareas de navegación y manipulación en entornos diseñados para seres humanos. Para las tareas de manipulación y agarre, se ha adquirido recientemente una mano robótica diseñada en la universidad de Gifu, Japón. Sin embargo, al comienzo de esta tesis, no se había realzado ningún trabajo destinado a la manipulación o el agarre de objetos. Por lo tanto, existe una motivación clara para investigar en este campo y ampliar las capacidades del robot, aspectos tratados en esta tesis. La primera parte de la tesis muestra la aplicación de un sistema de control de formaciones de robots en 3 dimensiones. El sistema explicado utiliza un esquema de tipo líder-seguidores, y se basa en la utilización del algoritmo Fast Marching Square para el cálculo de la trayectoria del líder. Después, mientras el líder recorre el camino, la formación se va adaptando al entorno para evitar la colisión de los robots con los obstáculos. El esquema de deformación presentado se basa en la información sobre el entorno previamente calculada con Fast Marching Square. El algoritmo es probado a través de distintas simulaciones en escenarios complejos. Los resultados son analizados estudiando principalmente dos características: cantidad de deformación necesaria y seguridad de los caminos de los robots. Aunque los resultados son satisfactorios en ambos aspectos, es deseable que en un futuro se realicen simulaciones más realistas y, finalmente, se implemente el sistema en robots reales. El siguiente capítulo nace de la misma idea, el control de formaciones de robots. Este concepto es usado para modelar el sistema brazo-mano del robot Manfred. Al igual que en el caso de una formación de robots, el sistema al completo incluye un número muy elevado de grados de libertad que dificulta la planificación de trayectorias. Sin embargo, la adaptación del esquema de control de formaciones para el brazo-mano robótico nos permite reducir la complejidad a la hora de hacer la planificación de trayectorias. Al igual que antes, el sistema se basa en el uso de Fast Marching Square. Además, se ha construido un esquema completo que permite modelar el entorno, calcular posibles posiciones para el agarre, y planificar los movimientos para realizarlo. Todo ello ha sido implementado en el robot Manfred, realizando pruebas de agarre con objetos reales. Los resultados muestran el potencial del uso de este esquema de control, dejando lugar para mejoras, fundamentalmente en el apartado de la modelización de objetos y en el cálculo y elección de los posibles agarres. A continuación, se trata de cerrar el lazo de control en el agarre de objetos. Una vez un sistema robótico ha realizado los movimientos necesarios para obtener un agarre estable, la posición final del objeto dentro de la mano resulta, en la mayoría de las ocasiones, distinta de la que se había planificado. Este hecho es debido a la acumulación de fallos en los sistemas de percepción y modelado del entorno, y los de planificación y ejecución de movimientos. Por ello, se propone un sistema Bayesiano basado en un filtro de partículas que, teniendo en cuenta la posición de la palma y los dedos de la mano, los datos de sensores táctiles y la forma del objeto, estima la posición del objeto dentro de la mano. El sistema parte de una posición inicial conocida, y empieza a ejecutarse después del primer contacto entre los dedos y el objeto, de manera que sea capaz de detectar los movimientos que se producen al realizar la fuerza necesaria para estabilizar el agarre. Los resultados muestran la validez del método. Sin embargo, también queda claro que, usando únicamente la información táctil y de posición, hay grados de libertad que no se pueden determinar, por lo que, para el futuro, resultaría aconsejable la combinación de este sistema con otro basado en visión. Finalmente se incluyen 2 anexos que profundizan en la implementación de la solución del algoritmo de Fast Marching y la presentación de los sistemas robóticos reales que se han usado en las distintas pruebas de la tesis.Programa Oficial de Doctorado en Ingeniería Eléctrica, Electrónica y AutomáticaPresidente: Carlos Balaguer Bernaldo de Quirós.- Secretario: Raúl Suárez Feijoo.- Vocal: Pedro U. Lim

Motion Coordination Problems with Collision Avoidance for Multi-Agent Systems

This chapter studies the collision avoidance problem in the motion coordination control strategies for multi-agent systems. The proposed control strategies are decentralised, since agents have no global knowledge of the goal to achieve, knowing only the position and velocity of some agents. These control strategies allow a set of mobile agents achieve formations, formation tracking and containment. For the collision avoidance, we add a repulsive vector field of the unstable focus type to the motion coordination control strategies. We use formation graphs to represent interactions between agents. The results are presented for the front points of differential-drive mobile robots. The theoretical results are verified by numerical simulation

Gaussian Processes for Machine Learning in Robotics

Mención Internacional en el título de doctorNowadays, machine learning is widely used in robotics for a variety of tasks such as perception, control, planning, and decision making. Machine learning involves learning, reasoning, and acting based on the data. This is achieved by constructing computer programs that process the data, extract useful information or features, make predictions to infer unknown properties, and suggest actions to take or decisions to make. This computer program corresponds to a mathematical model of the data that describes the relationship between the variables that represent the observed data and properties of interest. The aforementioned model is learned based on the available training data, which is accomplished using a learning algorithm capable of automatically adjusting the parameters of the model to agree with the data. Therefore, the architecture of the model needs to be selected accordingly, which is not a trivial task and usually depends on the machine-learning engineer’s insights and past experience. The number of parameters to be tuned varies significantly with the selected machine learning model, ranging from two or three parameters for Gaussian processes (GP) to hundreds of thousands for artificial neural networks. However, as more complex and novel robotic applications emerge, data complexity increases and prior experience may be insufficient to define adequate mathematical models. In addition, traditional machine learning methods are prone to problems such as overfitting, which can lead to inaccurate predictions and catastrophic failures in critical applications. These methods provide probabilistic distributions as model outputs, allowing for estimating the uncertainty associated with predictions and making more informed decisions. That is, they provide a mean and variance for the model responses. This thesis focuses on the application of machine learning solutions based on Gaussian processes to various problems in robotics, with the aim of improving current methods and providing a new perspective. Key areas such as trajectory planning for unmanned aerial vehicles (UAVs), motion planning for robotic manipulators and model identification of nonlinear systems are addressed. In the field of path planning for UAVs, algorithms based on Gaussian processes that allow for more efficient planning and energy savings in exploration missions have been developed. These algorithms are compared with traditional analytical approaches, demonstrating their superiority in terms of efficiency when using machine learning. Area coverage and linear coverage algorithms with UAV formations are presented, as well as a sea surface search algorithm. Finally, these algorithms are compared with a new method that uses Gaussian processes to perform probabilistic predictions and optimise trajectory planning, resulting in improved performance and reduced energy consumption. Regarding motion planning for robotic manipulators, an approach based on Gaussian process models that provides a significant reduction in computational times is proposed. A Gaussian process model is used to approximate the configuration space of a robot, which provides valuable information to avoid collisions and improve safety in dynamic environments. This approach is compared to conventional collision checking methods and its effectiveness in terms of computational time and accuracy is demonstrated. In this application, the variance provides information about dangerous zones for the manipulator. In terms of creating models of non-linear systems, Gaussian processes also offer significant advantages. This approach is applied to a soft robotic arm system and UAV energy consumption models, where experimental data is used to train Gaussian process models that capture the relationships between system inputs and outputs. The results show accurate identification of system parameters and the ability to make reliable future predictions. In summary, this thesis presents a variety of applications of Gaussian processes in robotics, from trajectory and motion planning to model identification. These machine learning-based solutions provide probabilistic predictions and improve the ability of robots to perform tasks safely and efficiently. Gaussian processes are positioned as a powerful tool to address current challenges in robotics and open up new possibilities in the field.El aprendizaje automático ha revolucionado el campo de la robótica al ofrecer una amplia gama de aplicaciones en áreas como la percepción, el control, la planificación y la toma de decisiones. Este enfoque implica desarrollar programas informáticos que pueden procesar datos, extraer información valiosa, realizar predicciones y ofrecer recomendaciones o sugerencias de acciones. Estos programas se basan en modelos matemáticos que capturan las relaciones entre las variables que representan los datos observados y las propiedades que se desean analizar. Los modelos se entrenan utilizando algoritmos de optimización que ajustan automáticamente los parámetros para lograr un rendimiento óptimo. Sin embargo, a medida que surgen aplicaciones robóticas más complejas y novedosas, la complejidad de los datos aumenta y la experiencia previa puede resultar insuficiente para definir modelos matemáticos adecuados. Además, los métodos de aprendizaje automático tradicionales son propensos a problemas como el sobreajuste, lo que puede llevar a predicciones inexactas y fallos catastróficos en aplicaciones críticas. Para superar estos desafíos, los métodos probabilísticos de aprendizaje automático, como los procesos gaussianos, han ganado popularidad. Estos métodos ofrecen distribuciones probabilísticas como salidas del modelo, lo que permite estimar la incertidumbre asociada a las predicciones y tomar decisiones más informadas. Esto es, proporcionan una media y una varianza para las respuestas del modelo. Esta tesis se centra en la aplicación de soluciones de aprendizaje automático basadas en procesos gaussianos a diversos problemas en robótica, con el objetivo de mejorar los métodos actuales y proporcionar una nueva perspectiva. Se abordan áreas clave como la planificación de trayectorias para vehículos aéreos no tripulados (UAVs), la planificación de movimientos para manipuladores robóticos y la identificación de modelos de sistemas no lineales. En el campo de la planificación de trayectorias para UAVs, se han desarrollado algoritmos basados en procesos gaussianos que permiten una planificación más eficiente y un ahorro de energía en misiones de exploración. Estos algoritmos se comparan con los enfoques analíticos tradicionales, demostrando su superioridad en términos de eficiencia al utilizar el aprendizaje automático. Se presentan algoritmos de recubrimiento de áreas y recubrimiento lineal con formaciones de UAVs, así como un algoritmo de búsqueda en superficies marinas. Finalmente, estos algoritmos se comparan con un nuevo método que utiliza procesos gaussianos para realizar predicciones probabilísticas y optimizar la planificación de trayectorias, lo que resulta en un rendimiento mejorado y una reducción del consumo de energía. En cuanto a la planificación de movimientos para manipuladores robóticos, se propone un enfoque basado en modelos gaussianos que permite una reducción significativa en los tiempos de cálculo. Se utiliza un modelo de procesos gaussianos para aproximar el espacio de configuraciones de un robot, lo que proporciona información valiosa para evitar colisiones y mejorar la seguridad en entornos dinámicos. Este enfoque se compara con los métodos convencionales de planificación de movimientos y se demuestra su eficacia en términos de tiempo de cálculo y precisión de los movimientos. En esta aplicación, la varianza proporciona información sobre zonas peligrosas para el manipulador. En cuanto a la identificación de modelos de sistemas no lineales, los procesos gaussianos también ofrecen ventajas significativas. Este enfoque se aplica a un sistema de brazo robótico blando y a modelos de consumo energético de UAVs, donde se utilizan datos experimentales para entrenar un modelo de proceso gaussiano que captura las relaciones entre las entradas y las salidas del sistema. Los resultados muestran una identificación precisa de los parámetros del sistema y la capacidad de realizar predicciones futuras confiables. En resumen, esta tesis presenta una variedad de aplicaciones de procesos gaussianos en robótica, desde la planificación de trayectorias y movimientos hasta la identificación de modelos. Estas soluciones basadas en aprendizaje automático ofrecen predicciones probabilísticas y mejoran la capacidad de los robots para realizar tareas de manera segura y eficiente. Los procesos gaussianos se posicionan como una herramienta poderosa para abordar los desafíos actuales en robótica y abrir nuevas posibilidades en el campo.Programa de Doctorado en Ingeniería Eléctrica, Electrónica y Automática por la Universidad Carlos III de MadridPresidente: Juan Jesús Romero Cardalda.- Secretaria: María Dolores Blanco Rojas.- Vocal: Giuseppe Carbon

Pause-and-Go Self-Balancing Formation Control of Autonomous Vehicles Using Vision and Ultrasound Sensors

In this work, we implement a decentralized and noncooperative state estimation and control algorithm to autonomously balance a team of robots in a circular formation pattern. The group of robots includes a leader periodically moving at a constant steering angle and a set of followers that, by only leveraging intermittent and noisy proximity measurements, independently implement a fully decentralized state estimation control algorithm to determine and adjust their relative position with closest neighbors. The algorithm is conducted in a pause-and-go sequence, where, during the pause, each robot stops to gather and process the information coming from the measurements, estimate the relative phase with respect to the others, and identify its closest pursuant. During the go, each robot accelerates to space from its closest pursuant and then to move at a constant speed when the desired spacing is achieved. The algorithm is tested in an unprecedented experiment on a custom-made low-cost caster-wheeled robotic framework featuring sonar and vision sensors mounted on a rotating platform to estimate the proximity distance to closer neighbors. The control scheme, which does not necessitate cooperation and is capable of coping with uncertain and intermittent sensor feedback data, is shown to be effective in balancing the robot on the circle even when, at a steady state, no feedback sensor data are available

Optimal Multi-UAV Trajectory Planning for Filming Applications

Teams of multiple Unmanned Aerial Vehicles (UAVs) can be used to record large-scale outdoor scenarios and complementary views of several action points as a promising system for cinematic video recording. Generating the trajectories of the UAVs plays a key role, as it should be ensured that they comply with requirements for system dynamics, smoothness, and safety. The rise of numerical methods for nonlinear optimization is finding a ourishing field in optimization-based approaches to multi- UAV trajectory planning. In particular, these methods are rather promising for video recording applications, as they enable multiple constraints and objectives to be formulated, such as trajectory smoothness, compliance with UAV and camera dynamics, avoidance of obstacles and inter-UAV con icts, and mutual UAV visibility. The main objective of this thesis is to plan online trajectories for multi-UAV teams in video applications, formulating novel optimization problems and solving them in real time. The thesis begins by presenting a framework for carrying out autonomous cinematography missions with a team of UAVs. This framework enables media directors to design missions involving different types of shots with one or multiple cameras, running sequentially or concurrently. Second, the thesis proposes a novel non-linear formulation for the challenging problem of computing optimal multi-UAV trajectories for cinematography, integrating UAV dynamics and collision avoidance constraints, together with cinematographic aspects such as smoothness, gimbal mechanical limits, and mutual camera visibility. Lastly, the thesis describes a method for autonomous aerial recording with distributed lighting by a team of UAVs. The multi-UAV trajectory optimization problem is decoupled into two steps in order to tackle non-linear cinematographic aspects and obstacle avoidance at separate stages. This allows the trajectory planner to perform in real time and to react online to changes in dynamic environments. It is important to note that all the methods in the thesis have been validated by means of extensive simulations and field experiments. Moreover, all the software components have been developed as open source.Los equipos de vehículos aéreos no tripulados (UAV) son sistemas prometedores para grabar eventos cinematográficos, en escenarios exteriores de grandes dimensiones difíciles de cubrir o para tomar vistas complementarias de diferentes puntos de acción. La generación de trayectorias para este tipo de vehículos desempeña un papel fundamental, ya que debe garantizarse que se cumplan requisitos dinámicos, de suavidad y de seguridad. Los enfoques basados en la optimización para la planificación de trayectorias de múltiples UAVs se pueden ver beneficiados por el auge de los métodos numéricos para la resolución de problemas de optimización no lineales. En particular, estos métodos son bastante prometedores para las aplicaciones de grabación de vídeo, ya que permiten formular múltiples restricciones y objetivos, como la suavidad de la trayectoria, el cumplimiento de la dinámica del UAV y de la cámara, la evitación de obstáculos y de conflictos entre UAVs, y la visibilidad mutua. El objetivo principal de esta tesis es planificar trayectorias para equipos multi-UAV en aplicaciones de vídeo, formulando novedosos problemas de optimización y resolviéndolos en tiempo real. La tesis comienza presentando un marco de trabajo para la realización de misiones cinematográficas autónomas con un equipo de UAVs. Este marco permite a los directores de medios de comunicación diseñar misiones que incluyan diferentes tipos de tomas con una o varias cámaras, ejecutadas de forma secuencial o concurrente. En segundo lugar, la tesis propone una novedosa formulación no lineal para el difícil problema de calcular las trayectorias óptimas de los vehículos aéreos no tripulados en cinematografía, integrando en el problema la dinámica de los UAVs y las restricciones para evitar colisiones, junto con aspectos cinematográficos como la suavidad, los límites mecánicos del cardán y la visibilidad mutua de las cámaras. Por último, la tesis describe un método de grabación aérea autónoma con iluminación distribuida por un equipo de UAVs. El problema de optimización de trayectorias se desacopla en dos pasos para abordar los aspectos cinematográficos no lineales y la evitación de obstáculos en etapas separadas. Esto permite al planificador de trayectorias actuar en tiempo real y reaccionar en línea a los cambios en los entornos dinámicos. Es importante señalar que todos los métodos de la tesis han sido validados mediante extensas simulaciones y experimentos de campo. Además, todos los componentes del software se han desarrollado como código abierto

Cooperative Control of Multiple Wheeled Mobile Robots: Normal and Faulty Situations

Recently, cooperative control of multiple unmanned vehicles has attracted a great deal of attention from scientific, industrial, and military aspects. Groups of unmanned ground, aerial, or marine vehicles working cooperatively lead to many advantages in a variety of applications such as: surveillance, search and exploration, cooperative reconnaissance, environmental monitoring, and cooperative manipulation, respectively. During mission execution, unmanned systems should travel autonomously between different locations, maintain a pre-defined formation shape, avoid collisions of obstacles and also other team members, and accommodate occurred faults and mitigate their negative effect on mission execution. The main objectives of this dissertation are to design novel algorithms for single wheeled mobile robots (WMRs) trajectory tracking, cooperative control and obstacle avoidance of WMRs in fault-free situations. In addition, novel algorithms are developed for fault-tolerant cooperative control (FTCC) with integration of fault detection and diagnosis (FDD) scheme. In normal/fault-free cases, an integrated approach combining input-output feedback linearization and distributed model predictive control (MPC) techniques is designed and implemented on a team of WMRs to accomplish the trajectory tracking as well as the cooperative task. An obstacle avoidance algorithm based on mechanical impedance principle is proposed to avoid potential collisions of surrounding obstacles. Moreover, the proposed control algorithm is implemented to a team of WMRs for pairing with a team of unmanned aerial vehicles (UAVs) for forest monitoring and fire detection applications. When actuator faults occur in one of the robots, two cases are explicitly considered: i) if the faulty robot cannot complete its assigned task due to a severe fault, then the faulty robot has to get out from the formation mission, and an FTCC strategy is designed such that the tasks of the WMRs team are re-assigned to the remaining healthy robots to complete the mission with graceful performance degradation. Two methods are used to investigate this case: the Graph Theory, and formulating the FTCC problem as an optimal assignment problem; and ii) if the faulty robot can continue the mission with degraded performance, then the other team members reconfigure the controllers considering the capability of the faulty robot. Thus, the FTCC strategy is designed to re-coordinate the motion of each robot in the team. Within the proposed scheme, an FDD unit using a two-stage Kalman filter (TSKF) to detect and diagnose actuator faults is presented. In case of using any other nonlinear controller in fault-free case rather than MPC, and in case of severe fault occurrence, another FTCC strategy is presented. First, the new reconfiguration is formulated by an optimal assignment problem where each healthy WMR is assigned to a unique place. Second, the new formation can be reconfigured, while the objective is to minimize the time to achieve the new formation within the constraints of the WMRs' dynamics and collision avoidance. A hybrid approach of control parametrization and time discretization (CPTD) and particle swarm optimization (PSO) is proposed to address this problem. Since PSO cannot solve the continuous control inputs, CPTD is adopted to provide an approximate piecewise linearization of the control inputs. Therefore, PSO can be adopted to find the global optimum solution. In all cases, formation operation of the robot team is based on a leader-follower approach, whilst the control algorithm is implemented in a distributed manner. The results of the numerical simulations and real experiments demonstrate the effectiveness of the proposed algorithms in various scenarios

Cooperative Control of Multiple Wheeled Mobile Robots: Normal and Faulty Situations

Recently, cooperative control of multiple unmanned vehicles has attracted a great deal of attention from scientific, industrial, and military aspects. Groups of unmanned ground, aerial, or marine vehicles working cooperatively lead to many advantages in a variety of applications such as: surveillance, search and exploration, cooperative reconnaissance, environmental monitoring, and cooperative manipulation, respectively. During mission execution, unmanned systems should travel autonomously between different locations, maintain a pre-defined formation shape, avoid collisions of obstacles and also other team members, and accommodate occurred faults and mitigate their negative effect on mission execution. The main objectives of this dissertation are to design novel algorithms for single wheeled mobile robots (WMRs) trajectory tracking, cooperative control and obstacle avoidance of WMRs in fault-free situations. In addition, novel algorithms are developed for fault-tolerant cooperative control (FTCC) with integration of fault detection and diagnosis (FDD) scheme. In normal/fault-free cases, an integrated approach combining input-output feedback linearization and distributed model predictive control (MPC) techniques is designed and implemented on a team of WMRs to accomplish the trajectory tracking as well as the cooperative task. An obstacle avoidance algorithm based on mechanical impedance principle is proposed to avoid potential collisions of surrounding obstacles. Moreover, the proposed control algorithm is implemented to a team of WMRs for pairing with a team of unmanned aerial vehicles (UAVs) for forest monitoring and fire detection applications. When actuator faults occur in one of the robots, two cases are explicitly considered: i) if the faulty robot cannot complete its assigned task due to a severe fault, then the faulty robot has to get out from the formation mission, and an FTCC strategy is designed such that the tasks of the WMRs team are re-assigned to the remaining healthy robots to complete the mission with graceful performance degradation. Two methods are used to investigate this case: the Graph Theory, and formulating the FTCC problem as an optimal assignment problem; and ii) if the faulty robot can continue the mission with degraded performance, then the other team members reconfigure the controllers considering the capability of the faulty robot. Thus, the FTCC strategy is designed to re-coordinate the motion of each robot in the team. Within the proposed scheme, an FDD unit using a two-stage Kalman filter (TSKF) to detect and diagnose actuator faults is presented. In case of using any other nonlinear controller in fault-free case rather than MPC, and in case of severe fault occurrence, another FTCC strategy is presented. First, the new reconfiguration is formulated by an optimal assignment problem where each healthy WMR is assigned to a unique place. Second, the new formation can be reconfigured, while the objective is to minimize the time to achieve the new formation within the constraints of the WMRs' dynamics and collision avoidance. A hybrid approach of control parametrization and time discretization (CPTD) and particle swarm optimization (PSO) is proposed to address this problem. Since PSO cannot solve the continuous control inputs, CPTD is adopted to provide an approximate piecewise linearization of the control inputs. Therefore, PSO can be adopted to find the global optimum solution. In all cases, formation operation of the robot team is based on a leader-follower approach, whilst the control algorithm is implemented in a distributed manner. The results of the numerical simulations and real experiments demonstrate the effectiveness of the proposed algorithms in various scenarios