Graphics Reference
In-Depth Information
Switch to the
next waypoint
Start
slowing
down
Waypoint 1
Waypoint 2
Waypoint 0
v
= 0
e
Vehicle
Fig. 4.11
Waypoint tracking logic. A cross-track controller minimizes the cross-track tracking error
e
↥
, while an along-track controller maintains a commanded velocity along the lines that connect
waypoints. The tracker switches waypoints when the along-track distance to the next waypoint
becomes smaller than a threshold. Waypoints are marked with
×
Start
slowing
down
Predicted
trajectory
Controller input
Root
Start states
Fig. 4.12
RRT with closed-loop dynamics. The
red lines
represent the input to the controller, which
are constructed by connecting a sample (marked with
) to the tree. The
blue lines
represent the
predicted vehicle trajectory, which is computed by the forward simulation
×
The main steps of the RRT algorithm are
1. Randomly sample the 3D world space.
2. Select a node in the tree.
3. Form a controller input from the selected node to the sample.
4. Forward simulate the closed-loop dynamics from the selected node, using the
controller input generated in step 3, and obtain a dynamically feasible trajectory.
5. Check if the predicted trajectory collides with or is occluded by obstacles
(Sect.
4.3.3
). If there is a collision, or the occlusion is in a close range, discard
the sample and go to step 1.
6. Add a sample and associated propagated trajectory to the tree. Also generate
intermediate nodes on the trajectory, so that it can branch off to another trajectory
for future samples.
