A sample code for Dubins path planning.
Dubins path is a analytical path planning algorithm for a simple car model.
It can generates a shortest path between two 2D poses (x, y, yaw) with maximum curvature constraint and tangent(yaw angle) constraint.
Generated paths consist of 3 segments of maximum curvature curves or a straight line segment.
Each segment type can be categorized by 3 types: 'Right turn (R)' , 'Left turn (L)', and 'Straight (S).'
Possible path will be at least one of these six types: RSR, RSL, LSR, LSL, RLR, LRL.
Dubins path planner can output each segment type and distance of each course segment.
For example, a RSR Dubins path is:
Each segment distance can be calculated by:
\alpha = mod(-\theta)
\beta = mod(x_{e, yaw} - \theta)
p^2 = 2 + d ^ 2 - 2\cos(\alpha-\beta) + 2d(\sin\alpha - \sin\beta)
t = atan2(\cos\beta - \cos\alpha, d + \sin\alpha - \sin\beta)
d_1 = mod(-\alpha + t)
d_2 = p
d_3 = mod(\beta - t)
where \theta is tangent and d is distance from x_s to x_e
A RLR Dubins path is:
Each segment distance can be calculated by:
t = (6.0 - d^2 + 2\cos(\alpha-\beta) + 2d(\sin\alpha - \sin\beta)) / 8.0
d_2 = mod(2\pi - acos(t))
d_1 = mod(\alpha - atan2(\cos\beta - \cos\alpha, d + \sin\alpha - \sin\beta) + d_2 / 2.0)
d_3 = mod(\alpha - \beta - d_1 + d_2)
You can generate a path from these information and the maximum curvature information.
A path type which has minimum course length among 6 types is selected, and then a path is constructed based on the selected type and its distances.
.. autofunction:: PathPlanning.DubinsPath.dubins_path_planner.plan_dubins_path