Hi everyone and welcome to our final video of the seventh module on motion planning for self-driving cars. In this lesson, we'll be introducing a method to generate a velocity profile for a given input path generated by our path planning algorithm. If you recall from Module one, there are many different factors that affect velocity profile generation. In particular, we will focus on the reference velocity provided by the behavior planner, the velocity of dynamic obstacles in front of us, and the velocity required to maintain passenger comfort and vehicle stability. By the end of this video, you should be able to construct velocity profiles along plan paths that take into account reference velocities from different behavior planning situations, time to collision to a leading dynamic obstacle, and comfort constraints. Finally, you should be able to implement a linear ramp velocity profile as well as a trapezoidal velocity profile. So let's get started. The first step in generating a velocity profile is determining our final required velocity. A good initial value for our final velocity is the reference velocity given to us by the behavior planner. This reference velocity will largely be influenced by the maneuver that the behavior plan selected based on the current driving scenario. For example, if we are stopped at an intersection and the light is still red, the behavior planner will issue a stopped maneuver to the local planner which will include outputting a zero velocity reference. If we're currently driving straight along a given road, the reference velocity may just be the speed limit of that current road. We will denote this by V ref going forward. The next input we take into consideration are the dynamic obstacle states. In particular, we focus on the lead vehicle in front of us. The lead vehicle speed regulates our speed, because if we exceed the lead vehicle speed, we will eventually collide with it. Recall that the time to collision is a function of our relative velocity to the lead vehicle as well as the length of the path to the lead vehicle given by s. Therefore, to preserve a safe time to collision, we will take our reference velocity to be the minimum of the lead vehicle speed and the behavior plan or reference speed. In addition, we need to make sure that we are below the lead vehicle speed before we reach their current location for this planning iteration because otherwise, we are still at risk for colliding with them. In general, when dealing with dynamic obstacles, it is good to leave space and time buffers in our calculations for safety. Therefore, if the end of our path lies ahead of the current position of the lead vehicle, we need to make sure that our final velocity is reached before that point including leaving a spatial buffer, as shown here by the red point along the path. By the time we reach the red point in our velocity profile, we will need to have reached the lead vehicle speed, if it is moving slower than us. The lead vehicle will move ahead by the time we reach the current position. At that point, we will have reached its speed preventing us from colliding with it. Note that we can also take lead vehicle tracking one step further and directly ensure a safe time to collision or separation distance given a lead vehicle speed, but this changes the control function to a relative distance tracking from a relative velocity tracking, so we'll stick to the velocity based approach in this video. The final input we consider is the maximum curvature along our planned path. Recall from our last lesson that when we sampled the optimized path, we record the curvature of the spiral at each point denoted by capital I. In addition, we mentioned in Module one, that there is a maximum lateral acceleration required to remain inside the comfort rectangle. The lateral acceleration as a function of the instantaneous curvature as well as the longitudinal speed along the curve. Therefore, the curvature bounds the longitudinal velocity that we can take while traversing our path. We can enforce this by ensuring the velocity stays below the required limit at each point on the path. However, if the curvature changes rapidly, we may not be able to achieve the required velocity while remaining within our longitudinal acceleration bounds. We therefore also find the maximum curvature across all points in our path and then find the associated maximum speed for that point. Our profile must then reach this required speed by the point in the path. We therefore define a deceleration to the required speed at the minimum point and an acceleration afterward. We can repeat this process for the next largest violation of the curvature constraint and so on until the velocity profile satisfies the curvature constraints along its length. A simpler approach that is useful for the assessments in this course is to identify the maximum curvature point, set the associated speed, and then simply maintain that speed until we pass the point. Since we are continuously re-planning in a receding horizon manner, once the high curvature point is reached, the new velocity profiles generated will naturally raise the speed based on the other objectives defined earlier. In essence, we can simplify the velocity profile generation process to the act of combining our curvature-based maximum speed with our previous two maximum speeds from the behavior planner and the lead vehicle by taking the minimum of all three as the desired final velocity of our profile. Next, we need to discuss what the shape of our velocity profile is going to be. One possible simple option is to generate a linear ramp profile from our current velocity to our desired final velocity that we calculated earlier. When planning our profile, we know the total arc length of our path, denoted s, and our initial and final speed. The first thing we need to calculate is our required acceleration, which we can solve for directly with our given inputs. We have to be careful to ensure that this calculated acceleration doesn't exceed our comfort rectangle as we discussed earlier in the course. If it does exceed the comfort rectangle, then we will need to clamp it. If an acceleration or deceleration is required that exceeds the comfort rectangle but is required due to safety concerns, such as during an emergency stop maneuver, then we can bypass this comfort rectangle to prevent a crash. If we do clamp our acceleration, then we need to update our final velocity accordingly using our maximum acceleration, a max, instead of our calculated acceleration, a. Once we have our acceleration profile, we can then calculate the velocity at each point along the path by looking at the arc length to the ith point. By iterating through the entire path and calculating the required velocity for each point, we have completely generated a velocity profile along our path to get to our desired end velocity. An alternative profile is the trapezoidal profile. It's useful when a car is approaching a stop sign, and we want to decelerate from our nominal speed to a lower transit speed before then decelerating again to a stop at the stop sign. For this velocity profile, we take as our input, our initial, and final velocities, our desired transit velocity, and our desired deceleration. This deceleration is usually chosen to be well within our comfort rectangle to make the profile as smooth as possible. The first step with this planner is to calculate the distance we will travel during our initial deceleration to our desired transit speed. This is the arc length traveled during the first segment of the trapezoidal profile and is given by the first equation here for Sa. From this, we know how much arc length along our initial path should be dedicated to our initial deceleration. The parameters for this initial calculation include our initial speed, vi, the transit speed, vt, and our gentle deceleration value, a naught. Once we have this arc length value, we can iterate through the points up to that arc length and use the second equation shown to calculate the required speed for the ith point. We can then repeat a similar process for the final deceleration from the transit velocity to rest at our stopped point. We will denote the entire arc length of our path as Sf. So the third segment of our profile has length Sf minus Sb. We can then solve for Sb as follows. Once we have Sb, we can then iterate through the points in this arc length range and assign them the required velocities for a gentle deceleration to a stop. The remaining points in the middle of the profile then take our constant transit speed, v sub t. Putting everything together, we have three regions in our velocity profile; an initial ramp down to our slow transit speed, a constant traversal at this transit speed, and a final ramp down to our stop point. We've shown you two methods here for generating a velocity profile, but there are many other options available as well. Using higher-order methods such as biquadratic velocity planners, we can minimize jerk along the trajectory as well. It's also possible to apply higher-order functions in our velocity ramp in the two methods we've shown here, which can generate smoother and more comfortable velocity profiles. Ultimately, velocity profiles can be optimized to meet multiple objectives simultaneously while satisfying comfort and safety constraints depending on the behavior to be executed. In developing a velocity profile generator for the final assessment, you should feel free to expand on any of the concepts discussed in this video, as seems appropriate. To summarize, in this video, we discussed how to incorporate the output reference velocity from our behavior planner into our velocity profile generation process. We also discussed how to use the time to collision to inform our velocity profile generation, and we incorporated lateral acceleration constraints as well. Finally, we discussed how to calculate linear ramp and trapezoidal velocity profiles to implement velocity transitions along a path. This completes the seventh module on motion planning. Let's summarize the main points. You first learn to work with two kinds of curves for path planning; splines and spirals. You then define the objectives and constraints needed to formulate the path planning problem. You developed experience with the psi pi optimize function and applied it to a conformal lattice planner to identify collision-free paths. Finally, you learned how to construct the velocity profile along the path to satisfy multiple constraints. You should now have enough knowledge to integrate a path planner and velocity profile planner to build your very own local planner from the ground up. You'll learn more about this in the final project in the next module. We'll see you there.
