Path Planning and Optimization

One of the most fascinating projects I have worked on eventually became US Patent… a system for multi-modal journey optimization. At first glance, it sounds straightforward: get a traveler from point A to point B as quickly as possible. But in reality, this is not a “shortest path” problem. It is a problem of navigating combinatorial explosion under uncertainty while still producing results that humans will actually use. The lesson was simple, but profound: a single “optimal” route is often the wrong answer. In practice, commuters do not blindly follow whatever the algorithm declares “fastest.” They balance hidden costs (number of transfers, reliability, waiting time) against raw travel time. A route that is one minute slower but has one fewer transfer will often be preferred. We approached this by abandoning the idea of returning just one solution. Instead, we designed an iterative search that keeps a fixed-length priority queue of candidate paths, pruning aggressively to keep the search tractable, but always preserving multiple high-quality alternatives. The output is a set of Pareto-efficient options: fast, but also different enough that a user can choose the one that fits their risk tolerance, comfort level, or schedule flexibility. This project shifted how I think about optimization. The real challenge isn’t mathematical purity, it is making decisions robust to the messiness of the real world. If the solution space is reduced to a single “optimal” point, you risk oversimplifying reality and delivering something no one wants to use. When we expose the trade-offs explicitly, we help people make better decisions.

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.

#Swarm of micro flying #drones #robots in the wild. This approach evolves aerial robotics in three aspects: capability of cluttered environment navigation, extensibility to diverse task requirements, and coordination as a swarm without external facilities. #Aerial #robots are widely deployed, but highly cluttered environments such as dense forests remain inaccessible to drones and even more so to swarms of drones. In these scenarios, previously unknown surroundings and narrow corridors combined with requirements of swarm coordination can create challenges. To enable swarm navigation in the wild, we develop miniature but fully autonomous drones with a trajectory planner that can function in a timely and accurate manner based on limited information from onboard sensors. The planning problem satisfies various task requirements including flight efficiency, obstacle avoidance, and inter-robot collision avoidance, dynamical feasibility, swarm coordination, and so on, thus realizing an extensible planner. Furthermore, the proposed planner deforms trajectory shapes and adjusts time allocation synchronously based on spatial-temporal joint optimization. A high-quality trajectory thus can be obtained after exhaustively exploiting the solution space within only a few milliseconds, even in the most constrained environment. The planner is finally integrated into the developed palm-sized swarm platform with onboard perception, localization, and control. Benchmark comparisons validate the superior performance of the planner in trajectory quality and computing time. Various real-world field experiments demonstrate the extensibility of our system. #paper: https://lnkd.in/dR7DP8Mt #github : https://lnkd.in/dwnM7yrq By: Xin Zhou, Xiangyong Wen, Zhepei Wang, Yuman Gao, Haojia Li, Qianhao Wang, Tiankai Yang, Haojian Lu, Yanjun Cao, Chao Xu, Fei Gao Zhejiang University #robotics #research #quadcopter #swarmintelligence #tech

I am delighted to share an interesting example of stabilizing a Car-like mobile robot (CMR) using a Nonlinear Model predictive controller (NMPC) optimal controller, to avoid obstacles and overcome barrier limitations. This is achieved by integrating the Artificial Potential Field (APF) method, with extended Kalman Filter (EKF) to estimate longitudinal and lateral position and drive the mobile robot to track a given trajectory while adhering to environmental constraints. CMR is typically modeled using its kinematic equations, capturing its nonholonomic constraints and motion characteristics.this often involves a bicycle model, where the robot is simplified to two wheels, a steerable front wheel and a fixed rear wheel. The state variables usually include the robot's position, orientation, and steering angle, while the control inputs are the linear velocity and Ang velocity.This model is essential for designing controllers. NMPC works by repeatedly solving an optimization problem over a finite prediction horizon at each control step. For a car-like robot, this involves using a dynamic model of the robot to predict its future states, such as position, orientation, and velocity, based on the current state and a sequence of control inputs. 'fmincon' solver in MATLAB solve this constrained nonlinear optimization, it aims to minimize a cost function that typically includes terms for trajectory tracking error, control effort, and adherence to constraints like obstacle avoidance, actuator limits, or road boundaries. By solving this problem in real-time, NMPC generates optimal control actions that drive the robot toward its goal while respecting system constraints and adapting to changes in the environment, if the robot detects an obstacle, NMPC can replan its trajectory to avoid collisions while still progressing toward the target. The integration of APF ensures smooth obstacle avoidance, while EKF provides accurate state estimation for robust control. This combination makes NMPC highly effective for CMRs operating in dynamic or uncertain environments, ensuring safe, efficient, and precise navigation. Overall, this approach showcases a powerful framework for autonomous navigation and control of CMR. In the future, the proposed framework for stabilizing a CMR can be further enhanced by exploring alternative techniques and solvers. For instance, Reinforcement Learning or Deep Learning could be incorporated to improve obstacle avoidance and trajectory planning in highly dynamic environments, enabling the robot to learn from experience and adapt to complex scenarios. solvers like IPOPT or CasADi could be tested alongside `fmincon` to improve computational efficiency and scalability, especially for large-scale problems. These advancements would not only improve the performance and robustness of the CMR but also expand its applicability to more challenging environments, such as urban autonomous driving or multi-robot coordination.

Can a single neural network policy generalize over poses, objects, obstacles, backgrounds, scene arrangements, in-hand objects, and start/goal states? Introducing Neural MP: A generalist policy for solving motion planning tasks in the real world 🤖 Quickly and dynamically moving around and in-between obstacles (motion planning) is a crucial skill for robots to manipulate the world around us. Traditional methods (sampling, optimization or search) can be slow and/or require strong assumptions to deploy in the real world. Instead of solving each new motion planning problem from scratch, we distill knowledge across millions of problems into a generalist neural network policy.  Our Approach: 1) large-scale procedural scene generation 2) multi-modal sequence modeling 3) test-time optimization for safe deployment Data Generation involves: 1) Sampling programmatic assets (shelves, microwaves, cubbys, etc.) 2) Adding in realistic objects from Objaverse 3) Generating data at scale using a motion planner expert (AIT*) - 1M demos! We distill all of this data into a single, generalist policy Neural policies can hallucinate just like ChatGPT - this might not be safe to deploy! Our solution: Using the robot SDF, optimize for paths that have the least intersection of the robot with the scene. This technique improves deployment time success rate by 30-50%! Across 64 real-world motion planning problems, Neural MP drastically outperforms prior work, beating out SOTA sampling-based planners by 23%, trajectory optimizers by 17% and learning-based planners by 79%, achieving an overall success rate of 95.83% Neural MP extends directly to unstructured, in-the-wild scenes! From defrosting meat in the freezer and doing the dishes to tidying the cabinet and drying the plates, Neural MP does it all! Neural MP generalizes gracefully to OOD scenarios as well. The sword in the first video is double the size of any in-hand object in the training set! Meanwhile the model has never seen anything like the bookcase during training time, but it's still able to safely and accurately place books inside it. Since, we train a closed-loop policy, Neural MP can perform dynamic obstacle avoidance as well! First, Jim tries to attack the robot with a sword, but it has excellent dodging skills. Then, he adds obstacles dynamically while the robot moves and it’s still able to safely reach its goal. This work is the culmination of a year-long effort at Carnegie Mellon University with co-lead Jiahui(Jim) Yang as well as Russell Mendonca, Youssef Khaky, Russ Salakhutdinov, and Deepak Pathak The model and hardware deployment code is open-sourced and on Huggingface!  Run Neural MP on your robot today, check out the following: Web: https://lnkd.in/emGhSV8k Paper: https://lnkd.in/eGUmaXKh Code: https://lnkd.in/e6QehB7R News: https://lnkd.in/enFWRvft

The robotics algorithm library every engineer should know! 📚 PythonRobotics is an open-source collection of Python code and textbook for robotics algorithms, created by Atsushi Sakai. It has 27.2k stars on GitHub and 7k forks, so it's a no brainer to bookmark it! 🔖 The project covers everything from localization (EKF, particle filters, histogram filters) to SLAM (FastSLAM, ICP matching), path planning (A*, RRT*, Dijkstra, D*, potential fields, state lattice), path tracking (Stanley, LQR, MPC), arm navigation, aerial navigation, and even bipedal planning. What makes this special? It's designed to be easy to read and understand, with minimal dependencies and practical, widely-used algorithms. Each algorithm comes with visual animations, mathematical explanations, and working code. The documentation is essentially a full textbook on robotics algorithms, available free online at https://lnkd.in/dvuuVy6e. Requirements are simple: Python 3.13+, NumPy, SciPy, Matplotlib, and cvxpy. That's it. This a learning resource with 2,201 commits, contributions from 138 developers, and active maintenance. The animations alone (stored in a separate repo) are worth studying. If you're learning robotics, building autonomous systems, or teaching algorithms, this is the resource. It's MIT licensed, so you can use it freely in research or commercial projects. Here's the link: https://lnkd.in/dNX5JzkX P.S. This is what good open-source looks like: educational, practical, well-documented, and community-driven. Bookmark it. 🔖 ~~ ♻️ Join the weekly robotics newsletter, and never miss any news → ziegler.substack.com

End-to-end motion planning simulation built on the CARLA simulator that seamlessly integrates advanced path planning, smooth trajectory generation, and real-time vehicle control 🚗 🔑 Key Features: - Path Planning with RRT*: I implemented sampling-based algorithms (RRT, RRT*, and Informed RRT*) to compute collision-free paths in a simulated urban environment. By dynamically sampling the CARLA world and avoiding obstacles, the planner finds viable routes even in challenging scenarios. - Smooth Trajectory Generation: Using motion primitives based on cubic polynomial interpolation, the system generates smooth trajectories between waypoints. This ensures that the vehicle’s motion is both safe and comfortable. - PID Control for Real-Time Vehicle Guidance: A combination of longitudinal and lateral PID controllers has been designed to accurately follow the planned trajectory. The vehicle’s throttle, brake, and steering commands are continuously updated in real-time. Repository: https://lnkd.in/d7qMqD-p 📖 Learn More: CARLA Open-source simulator for autonomous driving research: https://carla.org/ Robotic Path Planning RRT and RRT*: https://lnkd.in/dTqpGfJt The PID Controller & Theory Explained: https://lnkd.in/dMHYQdJB #autonomousdriving #CARLA #PathPlanning #PIDControl #Simulation #Robotics #RRT #RRT*

As someone working in network engineering with a foundation in operations research, I’ve always been fascinated by the intersection of optimization theory and real-world network performance. That’s why this new paper from Cornell University, “Breaking the Sorting Barrier for Directed Single-Source Shortest Paths”, caught my attention. The authors' present a deterministic algorithm that solves single-source shortest paths in a runtime that surpasses the classic Dijkstra’s algorithm. For decades, Dijkstra has been the gold standard. This result proves that, at least theoretically, there is still room for imporvement! Why it matters for us in networking: Routing efficiency: Shortest path problems underpin everything from OSPF and IS-IS to traffic engineering. Scalability: Faster algorithms for sparse graphs could have major implications for the massive topologies we see in modern networks. Theory to practice: Even if implementation is years out, breakthroughs like this shape how we think about designing future routing systems. This is a great example of how deep theory in graph algorithms and operations research can ripple out into the networks we build and run every day. 🔗 Read the paper here: https://lnkd.in/dknxccH5

Splitting a vehicle routing problem (VRP) into multiple subproblems (i.e. divide-and-conquer) can make it easier to solve, providing the split is good. In our ODL Live route optimiser engine we use splitting but get around the issues from bad splits using our ‘fuzzy splits’ algorithm, where the splits are temporary and only used for short periods before being replaced by a different set of splits. This lets us scale to large problems, e.g. scheduling 10K deliveries. With our current fuzzy splits algorithm, a single VRP is still only optimised on a single server. To solve very large problems (e.g. 100K stops), we need to be able to optimise a single VRP across multiple servers. To allow this, we’re currently developing a ‘fuzzy splits 2’ algorithm, designed for splitting a VRP across multiple servers. The big change is that ‘fuzzy splits 2’ must be asynchronous – if we split a VRP into 10 subproblems we can’t wait for each one to complete optimising before trying another set of splits. So the set of splits needs to evolve slowly – changing and diversifying over time (to investigate different subproblems) without modifying the splits for subproblems which are still running. Splits also need to be balanced (not too few jobs, not too many either) and not too diffuse (there’s little point putting deliveries in LA and New York in the same subproblem). This video below is from an experiment we’re running during the on-going development of our ‘fuzzy splits 2’ algorithm. The circles represent stops (e.g. deliveries) and are coloured according to their current split. The smaller grey circles are stops not currently assigned to any splits. #routing #VRP #optimization