A new optimization framework for robot motion planning

It isn’t easy for a robot to find its way out of a maze. Picture these machines trying to traverse a kid’s playroom to reach the kitchen, with miscellaneous toys scattered across the floor and furniture blocking some potential paths. This messy labyrinth requires the robot to calculate the most optimal journey to its destination, without crashing into any obstacles. What is the bot to do?

MIT CSAIL researchers’ algorithm Graphs of Convex Sets (GCS) Trajectory Optimization presents a scalable, collision-free motion planning system for these robotic navigational needs.

The approach marries graph search (a method for finding discrete paths in a network) and convex optimization (an efficient method for optimizing continuous variables so that a given cost is minimized), and can quickly find paths through maze-like environments while simultaneously optimizing the trajectory of the robot.

GCS can map out collision-free trajectories in as many as 14 dimensions (and potentially more), with the aim of improving how machines work in tandem in warehouses, libraries, and households.

The CSAIL-led project consistently finds shorter paths in less time than comparable planners, showing GCS’ capability to efficiently plan in complex environments.

In demos, the system skillfully guided two robotic arms holding a mug around a shelf while optimizing for the shortest time and path. The duo’s synchronized motion resembled a partner dance routine, swaying around the bookcase’s edges without dropping objects.

In subsequent setups, the researchers removed the shelves, and the robots swapped the positions of spray paints and handed each other a sugar box.

The success of these real-world tests shows the potential of the algorithm to aid in domains like manufacturing, where two robotic arms working in tandem could bring down an item from a shelf. Similarly, that duo could assist in putting books away in a household or library, avoiding the other objects nearby.

While problems of this nature were previously tackled with sampling-based algorithms, which can struggle in high-dimensional spaces, GCS uses fast convex optimization and can efficiently coordinate the work of multiple robots.

“Robots excel at repetitive, pre-planned motions in applications such as automotive manufacturing or electronics assembly but struggle with real-time motion generation in novel environments or tasks.

Previous state-of-the-art motion planning methods employ a ‘hub and spoke’ approach, using pre-computed graphs of a finite number of fixed configurations, which are known to be safe. During operation, the robot must strictly adhere to this roadmap, often leading to inefficient robot movements.

Motion planning using Graph-of-Convex-Sets (GCS) enables robots to easily adapt to different configurations within pre-computed convex regions—allowing the robot to ’round the corner’ as it makes its motion plans. By doing so, GCS allows the robot to rapidly compute plans within safe regions very efficiently using convex optimization.

“This paper presents a novel approach that has the potential to dramatically enhance the speed and efficiency of robot motions and their ability to adapt to novel environments,” says David M.S. Johnson, Co-founder and CEO Dexai Robotics.

GCS also thrived in simulation demos, where the team considered how a quadrotor could fly through a building without crashing into trees or failing to enter doors and windows at the correct angle. The algorithm optimized the path around the obstacles while simultaneously considering the rich dynamics of the quadrotor.

An optimal marriage

The recipe behind the MIT team’s success involves the marriage of two key ingredients: graph search and convex optimization. The first element of GCS searches graphs by exploring their nodes, calculating different properties at each one to find hidden patterns and identify the shortest path to reach the target. Much like the graph search algorithms used for distance calculation in Google Maps, GCS creates different trajectories to reach each point on its course toward its destination.

By blending graph search and convex optimization, GCS can find paths through intricate environments and simultaneously optimize the robot trajectory.

GCS executes this goal by graphing different points in its surrounding area and then calculating how to reach each one on the way to its final destination. This trajectory accounts for different angles to ensure the robot avoids colliding with the edges of its obstacles. The resulting motion plan enables machines to squeeze by potential hurdles, precisely maneuvering through each turn the same way a driver avoids accidents on a narrow street.

GCS was initially proposed in a 2021 paper as a mathematical framework for finding shortest paths in graphs where traversing an edge required solving a convex optimization problem.

Moving precisely across each vertex in large graphs and high-dimensional spaces, GCS had clear potential in robotic motion planning. In a follow-up paper, Marcucci and his team developed an algorithm applying their framework to complex planning problems for robots moving in high-dimensional spaces.

The 2023 article was featured on the cover of Science Robotics, while the group’s initial work is now published in the Society for Industrial and Applied Mathematics’ (SIAM) Journal on Optimization.

While the algorithm excels at navigating through tight spaces without collisions, there is still room to grow.The CSAIL team notes that GCS could eventually help with more involved problems where robots have to make contact with their environment, such as pushing or sliding objects out of the way. The team is also exploring applications of GCS trajectory optimization to robot task and motion planning.

“I’m very excited about this application of GCS to motion planning. But this is just the beginning. This framework is deeply connected to many core results in optimization, control, and machine learning, giving us new leverage on problems that are simultaneously continuous and combinatorial,” says Russ Tedrake, MIT Professor, CSAIL Principal Investigator, and co-author on a new paper about the work. “There is a lot more work to do.”