Trajectory Planning Algorithms

TRUST-Planner: Topology-guided Robust Trajectory Planner for AAVs with Uncertain Obstacle Spatial-temporal Avoidance Arxiv: https://lnkd.in/ezRgJgDc Video (not this paper): https://lnkd.in/eA4FDdwh How can Autonomous Aerial Vehicles (AAVs) safely navigate dense, dynamic environments where obstacles move unpredictably? TRUST-Planner introduces a topology-guided hierarchical framework that integrates global topological exploration with predictive, lightweight trajectory optimization—achieving 96% success rates in simulation and millisecond-level replanning in real-world flights. 🔁 At a Glance 💡 Goal: Overcome local minima, deadlocks, and collisions in dynamic AAV navigation by combining spatial-temporal topology guidance with robust trajectory optimization. ⚙️ Approach: Frontend (DEV-PRM): Dynamic Enhanced Visible Probabilistic Roadmap for diverse topological path exploration. Backend (UTF-MINCO): Uniform terminal-free minimum control polynomial + Dynamic Distance Field for predictive obstacle avoidance. Multi-branch Management: Incremental, parallel trajectory updates maintain multiple safe alternatives in real time. 📈 Impact (Key Metrics) 🧪 Simulation +81.9% efficiency in topological search vs. V-PRM. 96% success rate in cluttered, dynamic environments. Millisecond-level optimization (10 ms average per trajectory). 🤖 Real-World (Crazyflie 2.1 Nano Drone) Robust flights in static, dynamic, and adversarial penetration scenarios. Drone maintains near-max speed while avoiding multiple moving obstacles. 🔬 Experiments 🧪 Environments: Randomized static + dynamic obstacle fields, indoor motion capture trials. 🎯 Tasks: Navigation, obstacle avoidance, penetration through interceptors. 🦾 Robot: Crazyflie 2.1 with OptiTrack-based localization. 📐 Input: Position + velocity estimates of obstacles (EKF). 🛠 How to Implement 1️⃣ DEV-PRM Frontend – Uses Predictive Directional Cones + obstacle-aware sampling to capture spatio-temporal topology. 2️⃣ UTF-MINCO Backend – Lightweight unconstrained optimization with relaxed terminals + analytical gradients. 3️⃣ Incremental Multi-branch Framework – Parallel updating of main + alternative trajectories for real-time safety. 📦 Deployment Benefits ✅ Breaks out of local minima & deadlocks with multi-topology planning. ✅ Millisecond-level replanning for dynamic obstacle fields. ✅ Provides multiple safe fallback trajectories. ✅ Applicable to drones, autonomous vehicles, and dynamic robotics domains. Takeaway TRUST-Planner redefines AAV trajectory planning: not just finding a path, but managing diverse, safe futures in real time. Its blend of topology guidance + predictive optimization sets a new benchmark for robust aerial autonomy. Follow me to know more about AI, ML and Robotics!

Researchers at Hong Kong University MaRS Lab have just published another jaw dropping paper featuring their safety-assured high-speed aerial robot path planning system dubbed "SUPER". With a single MID360 lidar sensor they repeatedly achieved autonomous one-shot navigation at speeds exceeding 20m/s in obstacle rich environments. Since it only requires a single lidar these vehicles can be built with a small footprint and navigate completely independent of light, GPS and radio link. This is not just #SLAM on a #drone, in fact the SUPER system continuously computes two trajectories in each re-planning cycle—a high-speed exploratory trajectory and a conservative backup trajectory. The exploratory trajectory is designed to maximize speed by considering both known free spaces and unknown areas, allowing the drone to fly aggressively and efficiently toward its goal. In contrast, the backup trajectory is entirely confined within the known free spaces identified by the point-cloud map, ensuring that if unforeseen obstacles are encountered or if the system’s perception becomes uncertain, the system can safely switch to a precomputed, collision-free path. The direct use of LIDAR point clouds for mapping eliminates the need for time-consuming occupancy grid updates and complex data fusion algorithms. Combined with an efficient dual-trajectory planning framework, this leads to significant reductions in computation time—often an order of magnitude faster than comparable SLAM-based systems—allowing the MAV to operate at higher speeds without sacrificing safety. This two-pronged planning strategy is particularly innovative because it directly addresses the classic speed-safety trade-off in autonomous navigation. By planning an exploratory trajectory that pushes the speed envelope and a backup trajectory that guarantees safety, SUPER can achieve high-speed flight (demonstrated speeds exceeding 20 meters per second) without compromising on collision avoidance. If you've been tracking the progress of autonomy in aerial robotics and matching it to the winning strategies emerging in Ukraine, it's clear we're likely to experience another ChatGPT moment in this domain, very soon. #LiDAR scanners will continue to get smaller and cheaper, solid state VSCEL based sensors are rapidly improving and it is conceivable that vehicles with this capability can be built and deployed with a bill of materials below $1000. Link to the paper in the comments below.

2–6 DOF Robotic Manipulators Trajectory Tracking using PID in MATLAB ➡ Simulation of 2-DOF to 6-DOF robotic manipulators ➡ Detailed modeling of serial manipulators including UR5 ➡ Forward & Inverse Kinematics implementation for all DOF systems ➡ PID-based joint control for smooth and stable motion ➡ Trajectory tracking: Circle, Rectangle, and Infinity (∞) paths ➡ Real-time 3D visualization and animation in MATLAB ➡ Modular and well-structured code for scalability and learning ✨ Why this matters: Trajectory tracking is a fundamental problem in robotics, where a manipulator must precisely follow a desired path while maintaining stability and accuracy. This becomes increasingly complex as the number of degrees of freedom increases due to nonlinear kinematics, joint coupling, and control challenges. This project demonstrates how classical control techniques like PID can be effectively applied to multi-DOF robotic systems to achieve smooth and reliable motion. By integrating kinematic modeling with control strategies, the system reflects real-world industrial applications where robotic arms are required to perform precise tasks such as assembly, welding, and pick-and-place operations. 📊 Key Highlights: ✔ Complete kinematic modeling (FK & IK) for 2–6 DOF manipulators ✔ PID-based trajectory tracking for accurate motion control ✔ Implementation of multiple trajectories (circle, rectangle, infinity) ✔ Real-time simulation and visualization in MATLAB ✔ Clean and reusable code structure for educational use ✔ Industrial-level modeling with UR5 6-DOF manipulator 💡 Future Potential: This framework can be extended to: ➡ Advanced control (Adaptive, MPC, Fuzzy, AI-based control) ➡ Obstacle avoidance and path planning ➡ Integration with ROS 2 for real robot deployment ➡ Dynamic modeling and torque control ➡ Digital twin and industrial automation systems 🔗 For students, engineers & robotics enthusiasts: This project provides a complete hands-on approach to understanding robotic manipulators, control systems, and trajectory planning. It is ideal for learning how robotic arms achieve precise motion in real-world applications. 🔁 Repost to support robotics innovation & engineering learning! #Robotics #MATLAB #PIDControl #RobotManipulators #UR5 #ControlSystems #Automation #Mechatronics #EngineeringProjects #Simulation #STEM #EngineeringEducation

Built an autonomous drone mission planner that updates flight paths mid-air. Tested it on ArduPilot SITL and it actually worked. The goal was simple: program a drone to fly 15 waypoints, then, after the 10th one, make it fly perpendicular for 100m before continuing. All while the drone is already in the air. Getting DroneKit to talk to ArduPilot #SITL (Software In The Loop) was straightforward. The tricky part was calculating that perpendicular waypoint. Had to use Haversine formulas for distance and bearing, then inject it into an active mission via #MAVLink commands without the drone landing. Built this in Python with: - DroneKit for flight control - MAVLink protocol for waypoint commands - Real-time telemetry logging (distance, ETA, current position) - Dynamic mission updates mid-flight Also implemented a 3D path planning system for multiple drones. If 3 drones need to fly different routes, they can't be at the same point at the same time. Traditional A* doesn't handle this. Modified A*, where each grid point tracks occupation times. If drone A is at point (50,50,50) at t=10s, drone B either routes around it or delays. Tested on a 100×100×100 weighted grid with 3 simultaneous paths. Everything runs in simulation. ArduPilot SITL handles the #physics, #MAVProxy manages connections. Code is on GitHub with setup instructions for both systems. Links in comments. #Drones #Robotics #PathPlanning

"Fast Monte Carlo Tree Diffusion: 100x Speedup via Parallel Sparse Planning" by Jaesik Yoon, Hyeonseo Cho, Yoshua Bengio, Sungjin Ahn "Diffusion models have recently emerged as a powerful approach for trajectory planning. However, their inherently non-sequential nature limits their effectiveness in long-horizon reasoning tasks at test time. The recently proposed Monte Carlo Tree Diffusion (MCTD) offers a promising solution by combining diffusion with tree-based search, achieving state-of-the-art performance on complex planning problems. Despite its strengths, our analysis shows that MCTD incurs substantial computational overhead due to the sequential nature of tree search and the cost of iterative denoising. To address this, we propose Fast-MCTD, a more efficient variant that preserves the strengths of MCTD while significantly improving its speed and scalability. Fast-MCTD integrates two techniques: Parallel MCTD, which enables parallel rollouts via delayed tree updates and redundancy-aware selection; and Sparse MCTD, which reduces rollout length through trajectory coarsening. Experiments show that Fast-MCTD achieves up to 100x speedup over standard MCTD while maintaining or improving planning performance. Remarkably, it even outperforms Diffuser in inference speed on some tasks, despite Diffuser requiring no search and yielding weaker solutions. These results position Fast-MCTD as a practical and scalable solution for diffusion-based inference-time reasoning." Paper: https://lnkd.in/dZdx4hgE #machinelearning

Pleased to see the publication of the most recent work from MIT/ACL on "Off-Road Navigation via Implicit Neural Representation of Terrain Traversability", an excellent collaboration between Lucas (Yixuan Jia) and Andy (Qingyuan Li). https://lnkd.in/ej4aSgcm Autonomous off-road navigation requires robots to estimate terrain traversability from onboard sensors and plan motion accordingly. Conventional approaches typically rely on sampling-based planners such as MPPI to generate short-term control actions that aim to minimize traversal time and risk measures derived from the traversability estimates. These planners can react quickly but optimize only over a short look-ahead window, limiting their ability to reason about the full path geometry, which is important for navigating in challenging off-road environments. Moreover, they lack the ability to adjust speed based on the terrain-induced vibrations, which is important for smooth navigation on challenging terrains. In this paper, we introduce TRAIL (Traversability with an Implicit Learned Representation), an off-road navigation framework that leverages an implicit neural representation to model terrain properties as a continuous field that can be queried at arbitrary locations. This representation yields spatial gradients that enable integration with a novel gradient-based trajectory optimization method that adapts the path geometry and speed profile based on terrain traversability.

We just published this paper on IEEE Trans. on Vehicular Technology -x-x- Y. Zhang, J. Chen, H. Wei, M. G. Simões and H. Zhang, "Data-Driven Lyapunov-Based Model Predictive Control for Improved Trajectory Tracking in Multi-Wheel-Independent-Drive Electric Vehicle," in IEEE Transactions on Vehicular Technology, doi: 10.1109/TVT.2025.3587541 Abstract - This paper proposes a data-driven Lyapunov-based Model Predictive Control (LMPC) method for multi-wheel independent-drive electric vehicles to enhance the trajectory tracking accuracy while ensuring the vehicle stability. To improve the accuracy of the vehicle dynamics model, we first develop a temporal residual network to learn the residual between the nominal vehicle dynamics and the actual vehicle dynamics from a lot of training data offline. The temporal residual network predicts the vehicle dynamics residual online based on the vehicle states within a past time window. Then, by combining the nominal vehicle dynamics model with the temporal residual network, a more accurate compensation model is obtained. Building on this, we propose a novel data-driven control strategy specifically optimized for trajectory tracking. To ensure vehicle stability, a Lyapunov-based constraint based on the designed backstepping controller is incorporated into the data-driven LMPC. Subsequently, theoretical analysis is presented to validate the stability of the system. In the Carsim & Simulink co-simulation environment, we validated the effectiveness of the proposed temporal residual network and tracking control algorithm through open-loop and closed-loop simulations. keywords: Vehicle dynamics; Predictive models; Accuracy; Tires; Trajectory tracking; Residual neural networks; Neural networks; Wheels; Predictive control; Motors; Multi-wheel vehicle; Deep learning; Data-driven modeling; Lyapunov-based MPC; Trajectory tracking

The algorithm generates a linear trajectory by precisely controlling the acceleration of a robotic arm rigidly coupled to the horizontal surface, enabling the relative manipulation of an object as it slides on top of the surface. #research #paper: https://lnkd.in/dci6NUAX #authors: Hamidreza Raei, Elena De Momi, Arash Ajoudani, Istituto Italiano di Tecnologia Furthermore, two distinct algorithms have been developed to estimate the frictional forces dynamically during the sliding process. These algorithms provide online friction estimates after each action, which are fed back into the actor model as critical feedback after each action. This feedback mechanism enhances the policy's adaptability and robustness, ensuring more precise control of the platform's acceleration in response to varying surface condition. The proposed algorithm is validated through simulations and real-world experiments.

Can Drones Dodge Obstacles in a Split Second? "Rapid Collision Detection for Multicopter Trajectories" - This research introduces a rapid, continuous-time collision detection algorithm for multicopter trajectories that detects intersections with static and dynamic convex obstacles. - The algorithm is integrated with a multicopter trajectory generation method for fast obstacle-aware motion planning. - It tackles the challenge of navigating through nonconvex spaces created by convex obstacles, where feasible trajectories are harder to compute. - Monte Carlo simulations and experiments show the algorithm's ability to plan collision-free trajectories in milliseconds, ensuring real-time performance even in environments with moving obstacles. Video - https://lnkd.in/emiVqM2N Paper - https://lnkd.in/eWzVSJnN Code - https://lnkd.in/eZremZBF If you are an aspiring Roboticist, -------------------------------- Join my WhatsApp Robotics Channel - https://lnkd.in/dYxB9iCh Join our Robotics Community - https://lnkd.in/e6twxYJF Watch my Podcast - https://lnkd.in/eaX2yDSM -------------------------------- #robotics

Read about our work on "A Tree-based Next-best-trajectory Method for 3D UAV Exploration" published at the IEEE Transactions on Robotics from Björn Lindqvist, Akash Patel, Kalle Löfgren. Link: https://lnkd.in/dhA3EX99 This work presents a fully integrated tree-based combined exploration-planning algorithm: Exploration-RRT (ERRT). The algorithm is focused on providing real-time solutions for local exploration in a fully unknown and unstructured environment while directly incorporating exploratory behavior, robot-safe path planning, and robot actuation into the central problem. ERRT provides a complete sampling and tree-based solution for evaluating “where to go next” by considering a trade-off between maximizing information gain, and minimizing the distances travelled and the robot actuation along the path. The complete scheme is evaluated in extensive simulations, comparisons, as well as real-world field experiments in constrained and narrow subterranean and GPS-denied environments. The framework is fully ROS-integrated, straight-forward to use, and we open-source it at https://lnkd.in/dRruwyYh. #robotics #AI #autonomy #exploration