Welcome back. In this video, we'll discuss some of the most important constraints involved in motion planning. These constraints are often crucial for maintaining stability and comfort within the vehicle, as well as maintaining the safety of all agents in a given driving scenario. Specifically, you will learn about how the vehicle kinematic and dynamic models constrain our motion planning for the vehicle. You will learn about how static and dynamic obstacles impact our vehicle motion planning. Finally, you will see how regulatory elements impact the behaviors available to us during the motion planning process. Let's get started. The first of the motion planning constraints we will work with is related to the vehicle kinematics. In motion planning for autonomous driving, as we have discussed in course one, the kinematics for the ego vehicle are often simplified to what is known as the kinematic bicycle model. One reason why this model is chosen is that bicycles have a range of acceptable steering angle values similar to a car. For a fixed velocity V, this range of steering angle values and delta corresponds to a range of admissible curvatures, kappa that the vehicle can follow. This means that when performing motion planning while using the bicycle model, there is a maximum magnitude of curvature that can be executed when traversing a given path denoted by kappa max. The same holds for autonomous cars. This means that the curvature of any path we plan needs to be mindful of this maximum curvature. Unfortunately, this is what's known as a non-holonomic constraint. Intuitively, this means that the constraint doesn't depend on the only the state of the robot, but also on how the robot got to its current state. Non-holonomic constraints reduce the number of directions a robot can take at any given point in its workspace. In general, non-holonomic constraints make the planning problem much more complex. Now you may be wondering how does curvature impact the shape of a given path in our motion plan. To give you an intuitive understanding of what curvature is, here we have an arbitrary curve. For each point along the path, we can fit a circle to that point based on the instantaneous rates of change of the curve in space at that particular point. This is analogous to the idea of the instantaneous center of rotation we use to derive the bicycle model in the first place. Based on the shape of the plot, it appears different points are more curvy than others. So, our intuition should be the different points along the curve will have different curvatures. For example, here we have a circle fit to a particular point on the curve. As you can see, it has quite a large radius r, and intuitive interpretation of the curvature is that it's the inverse of the radius of the circle fit to that particular point along the curve. Since this radius is quite large, this point has relatively low curvature, which can be seen from the plot as the point has a more gentle bent. If instead we look at a second point on this curve, we can see that it has a smaller radius as it compared to the previous point. This in turn corresponds to a point of higher curvature and can be seen from the plot, this point has much more aggressive bend than the previous point. While this circle fitting approach is useful as a geometric interpretation, a more precise mathematical formulation is given here for the curvature. It can be computed using the first and second order derivatives of the x and y components of a given path defined with respect to arc length. These arc length derivatives are given by x prime, y prime, x double prime, and y double prime. The next constraint will be discussing is derived from the vehicle dynamics. These dynamic constraints focus on keeping the car in a stable safe state. The first of the dynamics constraints is imposed by what is called the friction ellipse of the car. If you recall, we discussed tire slip and the friction ellipse in our bicycle model during course one. The friction ellipse denotes the maximum magnitude of the friction forces that can be generated between a car tire and the road. If the applied forces of the car's engine exceed the friction forces of the tire, the tires will slip on the road surface. The turning functionality of a vehicle relies on the tires gripping the road surface. So to maintain control and stability, the car must remain within the friction circle. At the end of the day, this boils down to lateral and longitudinal acceleration constraints. In general, though the acceleration constraints are often much higher than what is physically comfortable for the passengers inside the car. Therefore, in non-emergency situations we often focus on the comfort rectangle of accelerations. These values constrain, the lateral and longitudinal accelerations to each lie within a range of comfort, denoted on the longitudinal acceleration along and lateral acceleration alat axis. This is shown here, where the comfort rectangle in blue lies well within the friction ellipse shown in green. This results in a tighter constraint on the feasible accelerations for a motion plan than the friction ellipse requires. From the lateral acceleration constraints, as well as the curvature of the path, we have now indirectly constrained the velocity of the car. The velocity of the car V is a function of the lateral acceleration of the car alat, as well as the instantaneous turning radius of the path r. Recall that the instantaneous curvature along the path kappa is the inverse of the turning radius of the car. If we now combine these equations, we can see that the square of the velocity is constrained by the maximum lateral acceleration, alat max which is a constant, as well as the curvature of the path kappa, which changes at each point along the path. Because of this, it's clear that when we are generating a velocity profile for our autonomous vehicle, we have to take the curvature of the path as well as the vehicles maximum lateral acceleration into consideration. Static obstacles also provide constraints upon our path planning process. Since static obstacles such as a parked car or a construction pylon have fixed positions that do not change with time, they are often modeled by blocking out certain portions of the ego vehicles workspace. This concept will be covered in greater detail in module two of this course, where we discuss the occupancy grid map. Essentially, static obstacles constrain the locations that a car can occupy along its path. There are numerous ways to perform static collision checking. One way is to check the swath of the vehicle as it travels along a given path. This swath is the union of all positions occupied by the body of the ego vehicle, as the ego vehicle traverses the path. If the swath overlaps with a static obstacle, that path is infeasible. Another option for fast collision checking is to approximate the body of the car with a set of circles, and calculate the circle locations as the car moves along its path. If a static obstacle lies within any of the circles, then the path is deemed infeasible. The union of these circles is often larger than the body of the vehicle, ensuring a conservative approximation. We'll be discussing this in more detail in future lessons. Dynamic obstacles on the other hand provides some of the most challenging constraints on the motion planning problem. Different classes of dynamic obstacles such as cars, bicycles, and pedestrians will all have different behaviors and motion models. Constraining our motion plan based on the behavior of these other agents will often involve prediction, which is subject to uncertainty. However, if we take the conservative approach and constrain ourselves to all possible behaviors of all agents, our motion planning problem quickly becomes over-constrained and impossible to solve meaningfully. The degree to which we balance between the safety of conservatism and the aggressiveness required for forward progress, is an active area of autonomous driving research. As a simple example, a case where dynamic obstacles will constrain our motion will be at an intersection. If two cars are entering the intersection orthogonal to one another, then we have a potential crash scenario. One way to check for whether a collision will occur, is to track the angle formed by the ego vehicles direction of travel and the vector from the ego vehicles location to the other agent's location. If this angle is unchanging as time progresses, then the ego vehicle will collide with the other agent, and our motion planner will need to decelerate to prevent this. Therefore, the dynamic obstacle forces us to modify our behavior based on how the obstacle proceeds in our driving scenario. Another example which we discussed in the previous lesson was when a leading vehicle is present in front of the ego vehicle. This leading vehicle places an upper constraint on the ego vehicles longitudinal velocity, for if we exceed their speed while remaining in the same lane we will eventually crash. As you can see, dynamic obstacles will constrain both our behavior planning process, where we make maneuver decisions, as well as our local planning process, where it will affect our velocity profile planning. We will be discussing this in further detail in subsequent modules. The final constraint that we will discuss encompasses the rules of the road as well as the regulatory elements present in the ego vehicles workspace. While the rules of the road provides some constraints to the planning problem, they also help us make informed decisions about other agents behaviors in the environment. For example, oncoming traffic is highly likely to stay in its lane, and not try to collide with our ego vehicle head on. This can help reduce the search space when trying to predict what other agents will do. One of the most common constraints imposed by the rules of the road are the lane constraints. The lane constraints simply put, are there to prevent our motion plan from leaving the ego vehicles current lane unless it is legal to do so. Lane constraints also inform the ego vehicle where it is safe to perform turning maneuvers as well. There are other soft rules of the road that we need to respect as well, such as maintaining a time gap between the ego vehicle and leading vehicles in our lane. The time gap is the amount of time that it would take for the ego vehicle to reach the leading vehicles current position while traveling at the ego vehicle's current speed. Maintaining a sizable time gap helps ensure safety during motion planning, by giving the ego vehicle ample reaction time to events in the workspace. The regulatory elements in the workspace will also impact our driving behavior. The ego vehicle clearly needs to respect red lights and stop signs to ensure safety, but also needs to be aware of speed limits in different areas and other dynamic regulatory elements, such as those presented by construction sites. In general, regulatory elements will have a large impact on which behaviors are available to us when performing motion planning, and will be discussed in further detail in module five. Now that we've discussed many of the autonomous driving motion planning constraints, let's summarize what we've learned so far. In this video, we first reviewed our bicycle model, and looked at how the models kinematics and dynamics, as well as the path curvature constrain our motion planning problem. Next, we looked at how static obstacles limit the locations our car can safely occupy, which restricts our feasible motion planning workspace, as well as how dynamic obstacles impact the maneuvers available to the ego vehicle and its velocity profile. Finally, we discussed the role of regulatory elements and how they affect our driving behaviors. Hopefully, this lesson has given you some insight into some of the constraints of the motion planning problem for autonomous driving. In our next video, we'll dive into some of the optimization objectives used for solving the motion planning problem. See you there.
