Shield AI Fundamentals: Trajectory Planning
Written by Kyle Carbon, Staff Engineer, Planning & Controls Lead.
We ask very difficult missions of Nova. For example, we ask it to explore a previously unknown building while detecting potential threats and evading them, respond to our user’s requests immediately, and to provide a live video stream and create a map in real time.
Completing these missions in a performant, robust manner requires Nova to plan out each step along the way. So, what exactly does planning mean in this context? To handle computational constraints, planning is typically decomposed into several levels: executive planning, path planning and trajectory planning.
Types of Planning
To understand these three aspects of planning, as well as controls which is closely related, and how they relate to Nova, consider the following comparison:
In the above context, a path refers to a list of positions that takes Nova from where it currently is to its desired destination. Notably, its path doesn’t say anything about when it must reach each point or what’s between each point; the path only states that it is possible to get from one point to the next.
Paths vs. Trajectories
While a path tells you that it is possible to reach your destination, it may not get you there in the safest or quickest manner; choosing the safest and quickest solution across large distances quickly becomes computationally prohibitive. To handle this, we turn to trajectory planning, which generates the desired position as a function of time, typically over a shorter time horizon (which makes this computationally feasible).
For Nova, because a trajectory is a function of time, the robot now also knows its desired velocity, acceleration, and its higher derivatives. An example trajectory would specify: “Nova must be at position X, with velocity V, with acceleration A, and so forth... at time T seconds into the future.” This is different from a path, which is simply a collection of positions in space and is not a function of time.
Optimization in Trajectory Planning
At Shield AI, there are a number of considerations Nova takes into account when optimizing its trajectory:
Safety is the number one consideration; it is imperative that Nova does not crash. This implies not only avoiding both static and dynamic obstacles, but also avoiding dangerous areas of the environment. For example, Nova will try to avoid stopping in cluttered areas.
Smoothness is also a high priority. It impacts our user’s trust in Nova — users express higher confidence in products that operate smoothly, therefore we make it a priority for Nova to fly gracefully. Perhaps surprisingly, smoothness also impacts safety. Highly irregular trajectories may be physically impossible to follow, which increases the likelihood of a crash.
There are many other factors that could be considered in trajectory planning, such as exploration speed and information gain, depending on the scenario.
These objectives are encoded in a cost function, and our trajectory planner chooses the safest, smoothest, and most informative trajectory that minimizes the cost function.
Generating a Trajectory
When in autonomous exploration mode, Nova navigates by searching for unknown space around it and planning a path on the global map to gradually eliminate all of the unknown space in the environment.
Nova plans a trajectory that balances safety, smoothness, and speed with respect to a local map. When planning, Nova leverages several key pieces of information, including its map of the environment, its current state, and its latest path. All of this information is required to generate safe, smooth, and fast trajectories. For instance, maps are important to trajectory planning because they synthesize various sensors to create a holistic view of the world, allowing Nova to plan intelligently; maps allow Nova to see and understand obstacles, such as tables, chandeliers, and windows.
Following a Trajectory Plan
Once Nova determines its trajectory plan, it knows how it must evolve its state over the next several seconds. At this point, the information contained in the trajectory is sent to the controller, which is responsible for achieving the desired state as a function of time. At a high level, the controller decides how fast to spin each of Nova’s motors, based on where it thinks it is (state estimation) and where it wants to go (planning).
When attempting to follow a trajectory, the controller deals with a variety of challenges, such as external disturbances (e.g. wind) and uncertainties (e.g. in its state estimate or its own system properties). For instance, a strong gust of wind might be difficult to overcome for a small quadrotor with limited motor and battery power. If the state estimate is uncertain, then it becomes difficult to control accurately -- if the quadrotor doesn’t know exactly where it is, it can’t guarantee it will reach the exact desired location.
Challenges of Trajectory Planning
Nova plans effective and robust trajectories. However, intelligent robotic systems such as Nova still face certain limitations with respect to trajectory planning and cannot yet plan quite as intelligently as a human.
Some notable challenges include:
Reacting to dynamic obstacles: Humans are extremely capable of intuiting how another object is moving. For Nova, and other engineered systems, reacting to dynamic obstacles is challenging, as it requires:
Low enough latencies in the sensor suite and system required to detect the obstacle;
Accurately and quickly predicting the behavior and trajectory of the obstacle;
Planning a dynamically feasible trajectory that maximizes the likelihood of avoiding a collision
Understanding and predicting an unknown environment: Humans reason quickly and intelligently about the world, then plan accordingly. However, things which humans see as obvious are not as simple for robots. For instance, a robot may need to slow down to safely round a corner. In contrast, a human can intuit that the hallway continues safely and continue at the same speed.
Planning with respect to changing dynamic limitations : Humans tend to have great body awareness and will plan exertion to match their energy. In contrast, a quadcopter must learn to slow down if its battery voltage is decreasing during the course of a flight.
Trajectory planning sits at the heart of guaranteeing the safety and reliability in which our customers trust and upon which they can rely. As such, Shield AI is committed to developing robust trajectory planning and continually improving Nova’s capabilities.