Hi everyone, and welcome to this lesson on reactive planning. In this video, we'll combine some of the knowledge we acquired from the two previous lessons, to develop a reactive motion planning algorithm known as the trajectory roll-out planner. This will introduce us to the task of trajectory planning, which will lay the groundwork for us to progress to more advanced planning methods presented later in module seven. By the end of this video, you should be able to implement the trajectory roll-out algorithm. This includes the trajectory propagation step, the collision checking step, and the path selection step in order to achieve the desired goal state. You should also understand how to apply the receding horizon concept to planning and the main drawbacks associated with this type of planner. At a high level, the trajectory roll-out planner uses the method of trajectory propagation discussed in lesson one to generate a set of candidate trajectories that the robot can follow from its current point in the workspace. We then take the obstacle information local to the robot and determine which paths are collision-free and which aren't. Of these collision-free paths, we then select the one that maximizes an objective function, which will include a term that rewards progress towards the goal. By performing this repeatedly, we end up with a receding horizon planner that reacts to the environment while making steady progress towards the goal. Let's go over each of these steps in more detail so you can implement this yourself. The first step of the algorithm is to generate the set of trajectories at each time step. How do we do this? For trajectory roll-out, each trajectory will correspond to applying a fixed input to the robot for multiple steps over a constant time horizon. We uniformly sample these fixed inputs across the range of available input values in order to generate a variety of potential candidate trajectories. By reaching a wide variety of trajectories across the input spectrum, we improve the quality of our trajectory search and our maneuverability as we are exploring a broader set of candidate paths for the robot to take. If we only use a small range of inputs, then our computation time improves. But there may be potential trajectory candidates that we miss out on in our computation, which could reduce the quality of the trajectory generated by our planner. However, sampling too many candidate trajectories means that we have additional computational overhead at each step as each additional trajectory will need to be generated, checked for collisions, and scored. Once we've chosen our set of inputs, we then need to generate the future states along the trajectory by propagating the state forward using the kinematic model of the vehicle as we discussed in lesson one. Recall that for the bicycle model, the two inputs are the velocity in the steering angle. If we hold the velocity constant, but vary the steering angle across the range minus pie over four to pie over four, we now have a set of arcs as our candidate trajectories. These arcs are generated by evaluating the kinematic equations recursively as we discussed in lesson one. Now that we have the set of arcs, we can check to see which ones are collision-free. For a collision checking algorithm, we're going to assume that we are given an occupancy grid that represents a discretization of the vehicles workspace. This discretization will be stored in the form of a matrix where each value of the matrix will denote if the corresponding position in the workspace is occupied or not. We can then perform collision checking using the swath-based method we discussed in the previous video. Recall that we generate the swath by sweeping the body of the robot along the path, and taking the union of all the footprints at each time step of a given trajectory. The footprint of the car will correspond to a set of indices in the occupancy grid. So each rotated and translated point along the path will also correspond to different indices in the occupancy grid. These indices will be stored in a set data structure to eliminate duplicates. We can then check each point of the swath to see which points of the swath overlap with occupied elements of the occupancy grid. We do this by iterating through each point in the swath set and checking the associated index in the occupancy grid for an obstacle. If any point in the swath is occupied, then that trajectory contains a collision. Once we've iterated through every trajectory generated in the previous step, we will be left with a set of collision-free kinematically feasible trajectories that we can then score using our objective function. The primary element that every objective function needs is some way of rewarding progress towards some goal point or region, which is the ultimate goal of our motion planning problem. A simple and effective way to do this is to have a term in the objective function that is proportional to the distance from the end of the candidate trajectory to the goal node. However, sometimes we will also want to encourage other behavior in our reactive planner. As we saw in module one, some examples of objective functions include minimizing the distance from the center line of a lane, we're penalizing the curvature of a path. Sometimes, we also want to reward paths that maximize the distance to the nearest obstacle, to maximize the flexibility of feasible paths available to future time steps in the planner. As we discussed in module one, there is no perfect objective function, and you will need to craft your own objective functions to fit your application. For our reactive planner, we will use the distance to the goal as our objective function to minimize, which is the first equation shown here. Once we have the objective function, we can iterate over the collision-free trajectories, and pick the path that maximizes the objective function, or minimizes the penalty depending on how you formulate the objective. Let's go over a concrete example to bring the whole algorithm together. Pictured here, we have the obstacles in the occupancy grid given in red, the goal region in yellow, and the initial point to the car at zero, zero, zero. Suppose our range of steering angles is between minus pie over four and pie over four with a pie over eight step size, and suppose we are using a constant velocity of 0.5 meters per second. In addition, let's assume our planner is using a time step of 0.1 seconds and each plan trajectory lasts for two seconds in total. If we apply our trajectory propagation algorithm that we outlined in lesson one using our bicycle kinematics model, we can then generate a set of paths for each selected steering angle in our steering range. The first trajectory has a steering angle of minus pie over four and as a result, we can see that the trajectory curves to the right. Our next trajectory has a steering angle of minus pie over eight and as you can see, the trajectory generated has a smaller curvature than the first one. Next, we have the zero steering angle trajectory which results in the car driving forward in a straight line. The positive steering angle trajectories are symmetrical to the negative steering angle trajectories and as expected, turn the car to the left. Now that we have a candidate set of trajectories, we need to check each one to see if they are collision-free using the collision checking algorithm we outlined in the last video, and reviewed earlier in this one. After translating and rotating the footprints along each trajectory, we check each occupancy grid index in the resulting swath to see if an obstacle is present. If any of the indices contain an obstacle, then that path is marked as having a collision, which we've denoted by the color red. All collision-free paths are colored green, which we can then evaluate using our objective function to find the best one. Recall that our objective function is the distance to the goal. The path that minimizes this distance to the goal is now colored in black. This completes our first planning iteration. At this point, we now have a trajectory for the vehicle to execute. However, we will not fully execute this trajectory before the next planning cycle. Instead, the vehicle will execute only the first few points of the cycle. The exact number depends on the planning frequency and our planning horizon will be shifted forward depending on our progress. This is exactly the receding horizon approach you applied to vehicle control in the first course in this specialization. Bringing this all together, at each time step, we plan a two-second trajectory, but only execute one second of it at a time. By doing this, the end time of our planning horizon shifts forward by one second for each planning cycle. This is known as a receding horizon planner because at each planning cycle we have a fixed planning time horizon whose n times slowly recedes towards the time point at which we reach the goal. This is illustrated here where the black is the portion of the trajectory that will be executed on the current cycle and the orange part is the leftover portion. Once the next planning cycle begins, we repeat the whole process again. We continue this process until we compute a trajectory that reaches the goal region, which we check at the end of each iteration. You can now see the rest of the steps that the planner took to the goal region in our example problem. One caveat to notice about this planner is that it is myopic. That is, it doesn't plan a path directly to the goal. It instead greedily sample sub-paths according to how close they get the robot to the goal. This can cause the planner to be shortsighted, to get stuck in dead ends, and in general, will cause the planner to find sub-optimal paths. However, this planner greatly reduces the complexity of the planning problem to the goal region and is fast enough that it can be used as an online planner. To summarize this video, we introduce the steps of the trajectory roll-out motion planning algorithm. Combining the concepts introduced in the first two lessons regarding trajectory propagation, as well as, collision checking. To cement this concept, we went over an example from this algorithm in action. Finally, we introduced the concept of receding horizon planners and discussed how they can be shortsighted when planning to the goal region. By now, you should have a good idea of how the trajectory roll-out algorithm works. In our next lesson, we'll be discussing dynamic windowing, and how it can help our trajectory roll-out algorithm generate more comfortable trajectories. Until then.
