Collision-free Trajectory Research Articles

Simultaneous Path Planning and Task Allocation in Dynamic Environments

This paper addresses the challenge of coordinating task allocation and generating collision-free trajectories for a fleet of mobile robots in dynamic environments. Our approach introduces an integrated framework comprising a centralized task allocation system and a distributed trajectory planner. The centralized task allocation system, employing a heuristic approach, aims to minimize the maximum spatial cost among the slowest robots. Tasks and trajectories are continuously refined using a distributed version of CHOMP (Covariant Hamiltonian Optimization for Motion Planning), tailored for multiple-wheeled mobile robots where the spatial costs are derived from a high-level global path planner. By employing this combined methodology, we are able to achieve near-optimal solutions and collision-free trajectories with computational performance for up to 50 robots within seconds.

Anatomical investigation of safety determining factors for keyhole drilling trajectories for robotic cochlear implant surgery.

Cochlear implants (CI) are the most successful bioprosthesis in medicine probably due to the tonotopic anatomy of the auditory pathway and of course the brain plasticity. Correct placement of the CI arrays, respecting the inner ear anatomy are therefore important. The ideal trajectory to insert a cochlear implant array is defined by an entrance through the round window membrane and continues as long as possible parallel to the basal turn of the cochlea. Image-guided surgery can directly drill with a robotic arm through the mastoid and execute an exact drilling of this precalculated most ideal trajectory. Here, we aim to identify critical anatomical structures determining the safest keyhole drilling trajectory by comparing easy and difficult CI surgeries performed with the HEARO Procedure: a robotic tool for image-guided surgery. Cone-beam computed tomography images of patients who underwent robot-assisted cochlear implantation surgery (RACIS) were included. Three of 25 cases had to be converted to conventional surgery because of the current safety mitigations based on anatomical distance. Radiological images in DICOM format were transferred to dedicated software (OTOPLAN® Cascination GMHB Bern Switzerland) for analyses. Surgical segmentation and previously planned trajectories were analyzed for these 25 cases by comparing cochlear sizes, facial recess sizes, round window sizes, and trajectory angles. In addition, facial recess angle, and cochlear orientation angles were measured with RadiAnt DICOM Viewer (Medixant, Pozan, Poland). Facial recess size, facial recess angle, and distance between the facial nerve and safe trajectory were smaller in patient who converted from robotic surgery to conventional. A significant positive correlation existed between basal turn angle and in-plane angle (p = 0.001, r = 0.859). In addition, there was a significant negative correlation between the basal turn length and the last electrode insertion depth degree (p = 0.007, r = 0.545). In our robotic surgery cases, we demonstrate that the limiting factors of anatomical relationships are constituted by the dimensions of the facial recess and the cochlear orientation. The findings of this study are considered to be a reference for future studies in achieving collision-free trajectory planning in robotic-assisted cochlear implant surgery.

A First-order Optimization Method for Cooperative Collision-free Trajectory Design for Multiple Unmanned Aerial Vehicles

A First-order Optimization Method for Cooperative Collision-free Trajectory Design for Multiple Unmanned Aerial Vehicles

Non-Euclidean motion planning with graphs of geodesically convex sets

Computing optimal, collision-free trajectories for high-dimensional systems is a challenging and important problem. Sampling-based planners struggle with the dimensionality, whereas trajectory optimizers may get stuck in local minima due to inherent nonconvexities in the optimization landscape. The use of mixed-integer programming to encapsulate these nonconvexities and find globally optimal trajectories has recently shown great promise, thanks in part to tight convex relaxations and efficient approximation strategies that greatly reduce runtimes. These approaches were previously limited to Euclidean configuration spaces, precluding their use with mobile bases or continuous revolute joints. In this paper, we handle such scenarios by modeling configuration spaces as Riemannian manifolds, and we describe a reduction procedure for the zero-curvature case to a mixed-integer convex optimization problem. We further present a method for obtaining approximate solutions via piecewise-linear approximations that is applicable to manifolds of arbitrary curvature. We demonstrate our results on various robot platforms, including producing efficient collision-free trajectories for a PR2 bimanual mobile manipulator.

Collision-Free Trajectory Optimization in Cluttered Environments Using Sums-of-Squares Programming

Collision-Free Trajectory Optimization in Cluttered Environments Using Sums-of-Squares Programming

COLREGs-Adaptive trajectory planning and decision-making in maritime autonomous surface ships

Decision-making for collision avoidance and trajectory planning are critical technologies for maritime autonomous surface ships. These systems must align with regulatory frameworks such as the International Regulations for Preventing Collisions at Sea (COLREGs) and account for the ship’s maneuvering capabilities for effective control and tracking. This study introduces a novel framework integrating regulatory consideration into the path-searching process, enhancing collision avoidance in both COLREG-compliant and non-compliant scenarios. The framework recasts the trajectory-planning problem into an optimal control problem and employs virtual obstacles and spatial–temporal navigation corridors consistent with collision-free decisions as constraints for trajectory optimization, improving navigation efficiency and comfort. The framework is validated through various encounter scenarios, and the results demonstrate that the proposed framework can produce superior collision avoidance decisions, while planning shorter navigation time and smoother collision-free trajectories, significantly improving the collision avoidance capability of maritime autonomous surface ships.

Mapping and Localization of Autonomous Mobile Robots in Simulated Indoor Environments

Autonomously making a map, localizing within it, and planning with it are fundamental problems in mobile robotics. Every autonomous mobile robot system must include a solution to all three problems. These three problems are interconnected, with simultaneous localization and mapping (SLAM) being a well-known issue. However, there is indeed a growing and developing realization in the research field that path planning how a robot goes about mapping and finding an environment (and then operating in the environment such as starting to the destination point) can avoid degenerate conditions and greatly reduce SLAM complexity. In this paper, the implementation of an autonomous mobile robot system for indoor environments using open-source ROS packages and a combination of cartography algorithm and adaptive Monte Carlo localization (AMCL) algorithms has been implemented. The system addresses the challenge of developing three components such as mapping, localization, and path planning systems for indoor autonomous mobile robots. The mapping module creates a global map using the cartography ROS package and SLAM algorithm. The localization module estimates the robot's pose using the AMCL approach. The planning module generates collision-free trajectories and control commands using the moving base ROS package. The experimental results demonstrate the effectiveness of this approach and its valuable contribution to the robotics field. The cartography algorithm mapping algorithm generates accurate and reliable maps, while the localization algorithm successfully determines the robot's position with good performance. Additionally, the path planning algorithm effectively avoids both static and dynamic obstacles, ensuring smooth navigation in the environment.

Collision-free trajectory tracking strategy of a UUV via finite-time extended state observer-based sliding mode predictive control

This paper focuses on a trajectory tracking problem for unmanned underwater vehicles (UUVs) subject to current disturbances and static obstacles. A double-loop framework is established. The kinematic governor employs model predictive control(MPC), which takes into account the UUV’s kinematic characteristics as well as the conditions for obstacle avoidance when calculating the control command and provides an optimal velocity command input for the dynamic controller. Then, the dynamic controller is developed based on a fast finite-time extended state observer (FESO) and a fast adaptive integral terminal sliding mode controller (FAITSMC). The construction of the fast FESO integrates UUV’s dynamics model, a proportional integral velocity variable, and a fractional order term of the output observation error, which can identify model uncertainties and external disturbances in finite time. By means of adaptive uncertainty compensation, the FAITSMC enables the error between actual speed and speed reference to converge to a minimum quickly. By applying Lyapunov stability theory and the finite-time analysis technique, sufficiency criteria are established to guide and keep the UUV on a reference trajectory via an inner-outer loop control structure. Finally, simulation examples in different scenarios are presented to verify the feasibility and effectiveness of the derived theoretical results.

A Two Phases Multiobjective Trajectory Optimization Scheme for Multi-UGVs in the Sight of the First Aid Scenario.

Timely delivery of first aid supplies is significant to saving lives when an accident happens. Among the promising solutions provided for such scenarios, the application of unmanned vehicles has attracted ever more attention. However, such scenarios are often very complex, while the existing studies have not fully addressed the trajectory optimization problem of multiple unmanned ground vehicles (multi-UGVs) against the scenario. This study focuses on multi-UGVs trajectory optimization in the sight of first aid supply delivery tasks in mass accidents. A two-stage completely decoupling fuzzy multiobjective optimization strategy is designed. On the first stage, with the proposed timescale involved tridimensional tunneled collision-free trajectory (TITTCT) algorithm, collision-free coarse tunnels are build within a tridimensional coordinate system, respectively, for the UGVs as the corresponding configuration space for a further multiobjective optimization. On the second stage, a fuzzy multiobjective transcription method is designed to solve the decoupled optimal control problem (OCP) within the configuration space with the consideration of priority constrains. Following the two-stage design, the computational time is significantly reduced when achieving an optimal solution of the multi-UGV trajectory planning, which is crucial in a first aid task. In addition, other objectives are optimized with the aspiration level reflected. Simulation studies and experiments have been curried out to testify the effectiveness and the improved computational performance of the proposed design.

Online dual robot–human collaboration trajectory generation by convex optimization

For dynamic collision-free trajectory planning in dual-robot and human collaborative tasks, this paper develops an online dual-robot Mutual Collision Avoidance (MCA) scheme based on convex optimization. A novel convex optimization formulation model, named Disciplined Convex programming by Shifting reference paths (DCS), is proposed for solving the single-robot trajectory optimization problem. Furthermore, a new dual-robot trajectory convex optimization algorithm is presented for online adjustment of the dual-robot trajectories according to the collaborative task priority. The overall pipeline, named DCS-MCA, generates collision-free and time-optimal dual-robot trajectories, while prioritizing the task accessibility of the high-priority robot. Simulation experiments demonstrate that DCS exhibits comparable performance to the current state-of-the-art single-robot motion planner, while the DCS-MCA outperforms common algorithms by up to 30% in time optimality for dual-robot collaborative tasks. The feasibility and dynamic performance of the proposed approach are further validated in a real collaborative cell, illustrating its suitability for collaborative dual-robot tasks in moderately dynamic environments.

A Hybrid Path Planning Technique for the Time-Efficient Navigation of Unmanned Vehicles in an Unconstrained Environment

Optimal path planning refers to choosing a collision-free, shortest, and smooth path, which is achieved by proposing a new technique, Optimized A* (OA*). The proposed OA* is the hybrid version of three techniques that are ABA* (Adaptive Bidirectional A*), VTM (Vector triangulation method), and PSF (Path smoothing filter which is the modification of Bezier curve technique). The conventional A* only deal with the heuristic values of concerned nodes, but the proposed ABA* algorithm also considers the occupied cells/ obstacles present near the starting and goal nodes in the grid cells to select the shortest path from start to destination. Due to the involvement of the occupied cells, the execution time, path curves, and the number of operations is primarily reduced because occupied nodes are considered as closed nodes, and its heuristic calculation is not considered in the conventional method. The trajectory generated by the proposed ABA* technique is also further made smoother by implementing a path smoothing filter that is the combination of VTM and modified Bezier curve technique based on selecting optimal control point for the collision free trajectory. While implementing the proposed OA* makes the path trajectory become smoother by reducing the number of sharp turns by 85.36% w.r.t traditional ACO, 73.91% w.r.t improved ACO, and 64.70% w.r.t hybrid solution of ACO+ A*. Due to the reduction in the number of sharp turns in the path trajectory, the acceleration of the mobile robot is increased by 52.96% w.r.t traditional ACO, 28.63% w.r.t improved ACO, and 19.96% w.r.t hybrid solution of ACO+ A*.

Collision-free trajectory planning for UAVs based on sequential convex programming

This paper proposes an optimization-based approach to solve the collision-free trajectory planning problem for unmanned aerial vehicles (UAVs) with nonlinear dynamical systems. The problem is modeled as a non-convex optimization problem, and then iteratively solved by sequential convex programming algorithm which approximates non-convex constraints by convexification methods. We linearize the dynamical equations of the UAV and penalize collision-free constraints with a hinge loss. Furthermore, a variable trust region method is employed to ensure the convergence and stability of the algorithm. We apply the algorithm to three-degree-of-freedom UAV case studies. Numerical results validate that our method can provide collision-free trajectories with a 27.19% reduction in total flight time and a 93.61% reduction in computation time on average compared with the genetic algorithm method.

Spatio-Temporal Joint Optimization-Based Trajectory Planning Method for Autonomous Vehicles in Complex Urban Environments.

Providing safe, smooth, and efficient trajectories for autonomous vehicles has long been a question of great interest in the field of autopiloting. In dynamic and ever-changing urban environments, safe and efficient trajectory planning is fundamental to achieving autonomous driving. Nevertheless, the complexity of environments with multiple constraints poses challenges for trajectory planning. It is possible that behavior planners may not successfully obtain collision-free trajectories in complex urban environments. Herein, this paper introduces spatio-temporal joint optimization-based trajectory planning (SJOTP) with multi-constraints for complex urban environments. The behavior planner generates initial trajectory clusters based on the current state of the vehicle, and a topology-guided hybrid A* algorithm applied to an inflated map is utilized to address the risk of collisions between the initial trajectories and static obstacles. Taking into consideration obstacles, road surface adhesion coefficients, and vehicle dynamics constraints, multi-constraint multi-objective coordinated trajectory planning is conducted, using both differential-flatness vehicle models and point-mass vehicle models. Taking into consideration longitudinal and lateral coupling in trajectory optimization, a spatio-temporal joint optimization solver is used to obtain the optimal trajectory. The simulation verification was conducted on a multi-agent simulation platform. The results demonstrate that this methodology can obtain optimal trajectories safely and efficiently in complex urban environments.

Implicit Swept Volume SDF: Enabling Continuous Collision-Free Trajectory Generation for Arbitrary Shapes

In the field of trajectory generation for objects, ensuring continuous collision-free motion remains a huge challenge, especially for non-convex geometries and complex environments. Previous methods either oversimplify object shapes, which results in a sacrifice of feasible space or rely on discrete sampling, which suffers from the "tunnel effect". To address these limitations, we propose a novel hierarchical trajectory generation pipeline, which utilizes the Swept Volume Signed Distance Field (SVSDF) to guide trajectory optimization for Continuous Collision Avoidance (CCA). Our interdisciplinary approach, blending techniques from graphics and robotics, exhibits outstanding effectiveness in solving this problem. We formulate the computation of the SVSDF as a Generalized Semi-Infinite Programming model, and we solve for the numerical solutions at query points implicitly, thereby eliminating the need for explicit reconstruction of the surface. Our algorithm has been validated in a variety of complex scenarios and applies to robots of various dynamics, including both rigid and deformable shapes. It demonstrates exceptional universality and superior CCA performance compared to typical algorithms. The code will be released at https://github.com/ZJU-FAST-Lab/Implicit-SVSDF-Planner for the benefit of the community.

Collision-Free Trajectory Planning Optimization Algorithms for Two-Arm Cascade Combination System

As a kind of space robot, the two-arm cascade combination system (TACCS) has been applied to perform auxiliary operations at different locations outside space cabins. The motion coupling relation of two arms and complex surrounding obstacles make the collision-free trajectory planning optimization of TACCS more difficult, which has become an urgent problem to be solved. For the above problem, this paper proposed collision-free and time–energy–minimum trajectory planning optimization algorithms, considering the motion coupling of two arms. In this method, the screw-based inverse kinematics (IK) model of TACCS is established to provide the basis for the motion planning in joint space by decoupling the whole IK problem into two IK sub-problems of two arms; the minimum distance calculation model is established based on the hybrid geometric enveloping way and basic distance functions, which can provide the efficient and accurate data basis for the obstacle-avoidance constraint condition of the trajectory optimization. Moreover, the single and bi-layer optimization algorithms are presented by taking motion time and energy consumption as objectives and considering obstacle-avoidance and kinematics constraints. Finally, through example cases, the results indicate that the bi-layer optimization has higher convergence efficiency under the premise of ensuring the optimization effect by separating variables and constraint terms. This work can provide theoretical and methodological support for the efficient and intelligent applications of TACCS in the space arena.

Adaptive Meta-heuristic Framework for Real-time Dynamic Obstacle Avoidance in Redundant Robot Manipulators

Robotic manipulator faces a challenge in navigating dynamic environments while ensuring collision-free trajectories, especially for redundant manipulators. Inverse kinematics involves finding joint angles to reach a specific point in 3D space. The shift from classical analytical and numerical methods to optimization heuristic algorithms is driven by the increasing complexity of robotic systems and the demand for more versatile and adaptive solutions. Meta-heuristic algorithms offer a transformative approach by framing the inverse kinematics problem as an optimization challenge, providing a flexible and robust means to navigate complex solution spaces. Metaheuristic algorithms, known for their ability to explore high-dimensional search spaces and avoid local optima, offer robust solutions for these challenges. They enhance computational efficiency, enabling real-time decision-making and obstacle avoidance, making them ideal for complex robotic applications. These characteristics of the metaheuristic algorithms can used in developing an integrated framework that offers complete solution to robot manipulators. This research article presents a generalized framework leveraging meta-heuristic algorithms to address dynamic obstacle avoidance in redundant manipulators. The framework uses meta-heuristic algorithms as the inverse kinematics solver, 3D trajectory planner, and obstacle avoidance mechanism, encompassing both static and dynamic obstacles. The proposed framework is generalized and gives the user to select the type of robot manipulator, with any number of links with any custom trajectory within the workspace of the robot manipulator. Also, any metaheuristic algorithm can be used in the proposed framework. The proposed framework is implemented in MATLAB’s app designer for simulation with six different meta-heuristic algorithms. The effectiveness of the framework was evaluated in terms of its capability to generate 3d path, its ability to follow generated trajectory, while seamlessly adapting to dynamically changing environments. Through simulation, the framework showcased robust performance in navigating workspaces with moving obstacles, ensuring collision-free motion for redundant manipulators.

Fast generation of collision-free low-thrust trajectories for asteroid landing using deep neural networks

This study presents a fast generation method of time-optimal low-thrust trajectories based on deep neural networks (DNN) for the collision-free asteroid landings. An equivalent unconstrained differentiable problem is transformed via the introduction of smooth penalty function, so that it can be addressed with the optimal control framework. To efficiently solve the equivalent problem to generate sufficient dataset for training DNNs, a double homotopy method is developed. Specifically, the equivalent problem is connected to the time-optimal control problem without anti-collision through two consecutive simplifications, and a solving process with double backward continuation is also formulated. Besides, two DNN models are constructed to provide high-quality predictions for the minimum glide-slope constraint angle of an initial state, and the initial costates and landing time for time-optimal low-thrust trajectories with anti-collision constraints, respectively. As such, the efficiency of collision-free low-thrust landing trajectory generation is improved significantly. For the cases within the range of training dataset or within a certain extension, the low-thrust landing trajectories with a very small terminal landing error can be generated using the predicted initial costates. Meanwhile, with the high-quality initial costates, the high-precision landing trajectories can be obtained by the double homotopy method in only a few iterative steps. Finally, the promising results in a Bennu collision-free asteroid landing scenario validate the efficiency of the proposed method.

Efficient Motion Primitives-Based Trajectory Planning for UAVs in the Presence of Obstacles

The achievement of full autonomy in Unmanned Aerial Vehicles (UAVs) is significantly dependent on effective motion planning. Specifically, it is crucial to plan collision-free trajectories for smooth transitions from initial to final configurations. However, finding a solution executable by the actual system adds complexity: the planned motion must be dynamically feasible. This involves meeting rigorous criteria, including vehicle dynamics, input constraints, and state constraints. This work addresses optimal kinodynamic motion planning for UAVs in the presence of obstacles by employing a hybrid technique instead of conventional search-based or direct trajectory optimization approaches. This technique involves precomputing a library of motion primitives by solving several Two-Point-Boundary-Value Problems (TPBVP) offline. This library is then repeatedly used online within a graph-search framework. Moreover, to make the method computationally tractable, continuity between consecutive motion primitives is enforced only on a subset of the state variables. This approach is compared with a state-of-the-art quadrotor-tailored search-based approach, which generates motion primitives online through control input discretization and forward propagation of the dynamic equations of a simplified model. The effectiveness of both methods is assessed through simulations and real-world experiments, demonstrating their ability to generate resolution-complete, resolution-optimal, collision-free, and dynamically feasible trajectories. Finally, a comparative analysis highlights the advantages, disadvantages, and optimal usage scenarios for each approach.

Trajectory planning and control of spacecraft avoiding dynamic debris swarm

The continuous accumulation of space debris poses a significant threat to spacecraft on orbit. This paper introduces a dynamic trajectory planning and control framework aimed at ensuring the safe traversal of spacecraft through space debris swarms. Initially, a threat model is established, considering the movement of the debris swarm and the constraints of spacecraft detection range. This model utilizes a dynamically updated, redefined Gaussian mixture model to characterize the threat posed by the debris swarm to spacecraft, with proximity to debris correlating to increased threat levels. Building upon this threat model, four guidance rules are defined, including target guidance, minimal threat, threat threshold, and local path optimization, to facilitate the generation of the shortest collision-free local trajectory for spacecraft. Combining the concept of receding horizon control, an online real-time planning and control framework is formulated for spacecraft navigating through debris swarms. Finally, numerical and semi-physical simulations verify that the proposed framework can stably generate spacecraft safe trajectories and control commands under the threat of numerous dynamic space debris, showing the characteristics of optimal planning trajectory, rapid computation speed and robust safety features.

Decentralized Navigation with Optimality for Multiple Holonomic Agents in Simply Connected Workspaces.

Multi-agent systems are utilized more often in the research community and industry, as they can complete tasks faster and more efficiently than single-agent systems. Therefore, in this paper, we are going to present an optimal approach to the multi-agent navigation problem in simply connected workspaces. The task involves each agent reaching its destination starting from an initial position and following an optimal collision-free trajectory. To achieve this, we design a decentralized control protocol, defined by a navigation function, where each agent is equipped with a navigation controller that resolves imminent safety conflicts with the others, as well as the workspace boundary, without requesting knowledge about the goal position of the other agents. Our approach is rendered sub-optimal, since each agent owns a predetermined optimal policy calculated by a novel off-policy iterative method. We use this method because the computational complexity of learning-based methods needed to calculate the global optimal solution becomes unrealistic as the number of agents increases. To achieve our goal, we examine how much the yielded sub-optimal trajectory deviates from the optimal one and how much time the multi-agent system needs to accomplish its task as we increase the number of agents. Finally, we compare our method results with a discrete centralized policy method, also known as a Multi-Agent Poli-RRT* algorithm, to demonstrate the validity of our method when it is attached to other research algorithms.