diff --git a/PathTracking/vector_pursuit/vector_pursuit.py b/PathTracking/vector_pursuit/vector_pursuit.py new file mode 100644 index 0000000000..0c4b716e67 --- /dev/null +++ b/PathTracking/vector_pursuit/vector_pursuit.py @@ -0,0 +1,201 @@ +"""Forward path tracking with Vector Pursuit and a kinematic bicycle model. + +Author: OpenAI Codex +Reference: J. Wit, C. D. Crane III, D. Armstrong, Autonomous Ground Vehicle +Path Tracking, Journal of Robotic Systems 21(8), 439-449, 2004. +https://doi.org/10.1002/rob.20031 + +Positions are at the rear axle in meters, yaw/steering are radians, and +velocity is m/s. This example tracks a smooth, non-self-intersecting path; +it does not plan around obstacles or drive in reverse. +""" + +from dataclasses import dataclass, replace +import math +import pathlib +import sys + +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation +import numpy as np + +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) +from PathPlanning.CubicSpline import cubic_spline_planner +from utils.angle import angle_mod + +WHEELBASE = 2.9 # [m] +MAX_STEER = math.radians(45.0) +LOOK_AHEAD = 4.0 # [m] measured along the sampled path +TIME_RATIO = 2.0 # k = rotation correction time / translation time +TARGET_SPEED = 2.0 # [m/s] +SPEED_GAIN = 1.0 # [1/s] +GOAL_TOLERANCE = 0.3 # [m] +DT = 0.05 # [s] +MAX_TIME = 80.0 # [s] +PATH_SPACING = 0.1 # [m] spline parameter increment +show_animation = True + + +@dataclass +class State: + """Rear-axle position (m), yaw (rad), and forward speed (m/s).""" + + x: float = 0.0 + y: float = -2.0 + yaw: float = 0.0 + speed: float = 0.0 + + def update(self, acceleration, steering): + """Apply acceleration (m/s²) and bounded steering (rad) for DT seconds.""" + steering = float(np.clip(steering, -MAX_STEER, MAX_STEER)) + self.x += self.speed * math.cos(self.yaw) * DT + self.y += self.speed * math.sin(self.yaw) * DT + self.yaw = angle_mod(self.yaw + self.speed / WHEELBASE * math.tan(steering) * DT) + self.speed += acceleration * DT + + +def vector_pursuit_curvature(target_x, target_y, heading_error, time_ratio=TIME_RATIO): + """Return signed curvature (1/m) for a target in the rear-axle frame. + + The target must be strictly ahead (target_x > 0, meters). heading_error + is target yaw minus vehicle yaw (radians); time_ratio is the positive + dimensionless k in the paper. Positive curvature turns left. + + The signed translation arc angle is beta = 2*atan2(target_y, target_x). + Its length is d / sinc(beta/2). Combining translation and rotation + screws gives ((k - 1)*beta + heading_error) / (k * arc_length). + This form includes the straight-ahead limit without dividing by beta. + """ + if time_ratio <= 0.0: + raise ValueError("time_ratio must be positive") + if target_x <= 0.0: + raise ValueError("Vector Pursuit requires a target strictly ahead") + heading_error = angle_mod(heading_error) + chord = math.hypot(target_x, target_y) + arc_angle = 2.0 * math.atan2(target_y, target_x) + # NumPy's sinc uses pi*x, whereas the geometric expression uses sin(x)/x. + arc_length = chord / np.sinc(arc_angle / (2.0 * math.pi)) + return ((time_ratio - 1.0) * arc_angle + heading_error) / (time_ratio * arc_length) + + +def steering_control(state, target, time_ratio=TIME_RATIO): + """Return bounded bicycle steering toward a world-frame [x, y, yaw] target.""" + dx, dy = target[0] - state.x, target[1] - state.y + cosine, sine = math.cos(state.yaw), math.sin(state.yaw) + local_x = cosine * dx + sine * dy + local_y = -sine * dx + cosine * dy + curvature = vector_pursuit_curvature(local_x, local_y, + target[2] - state.yaw, time_ratio) + return float(np.clip(math.atan(WHEELBASE * curvature), -MAX_STEER, MAX_STEER)) + + +def search_target_index(state, course, previous_index=0): + """Find a forward path sample LOOK_AHEAD meters beyond the nearest sample. + + course contains rows [x (m), y (m), tangent yaw (rad)] ordered along the + path. The returned nearest index never precedes previous_index. Target + selection follows sampled arc length and is clamped to the final sample. + """ + distances = np.hypot(course[previous_index:, 0] - state.x, + course[previous_index:, 1] - state.y) + nearest_index = previous_index + int(np.argmin(distances)) + target_index = nearest_index + distance = 0.0 + while target_index < len(course) - 1 and distance < LOOK_AHEAD: + distance += np.linalg.norm(course[target_index + 1, :2] - course[target_index, :2]) + target_index += 1 + return target_index, nearest_index + + +def create_course(): + """Sample an S-shaped cubic-spline path with tangent headings.""" + waypoint_x = [0.0, 10.0, 20.0, 30.0, 40.0, 50.0, 60.0] + waypoint_y = [0.0, 0.0, 8.0, -8.0, 6.0, 0.0, 0.0] + x, y, yaw, _, _ = cubic_spline_planner.calc_spline_course( + waypoint_x, waypoint_y, ds=PATH_SPACING) + return np.column_stack((x, y, yaw)) + + +def simulate(course=None, initial_state=None, max_time=MAX_TIME): + """Track a course and return it with state, target, and steering histories. + + History rows are [time (s), x (m), y (m), yaw (rad), speed (m/s)]. + The initial state is copied. Reaching the final look-ahead sample does + not end the run; the rear axle must be within GOAL_TOLERANCE of the goal. + A timeout raises RuntimeError instead of reporting successful tracking. + """ + if course is None: + course = create_course() + state = State() if initial_state is None else replace(initial_state) + history = [[0.0, state.x, state.y, state.yaw, state.speed]] + targets, steering_history = [], [] + nearest_index = 0 + for step in range(int(max_time / DT) + 1): + goal_distance = math.hypot(course[-1, 0] - state.x, course[-1, 1] - state.y) + if goal_distance <= GOAL_TOLERANCE: + return course, np.array(history), np.array(targets), np.array(steering_history) + if step == int(max_time / DT): + break + target_index, nearest_index = search_target_index(state, course, nearest_index) + steering = steering_control(state, course[target_index]) + target_speed = min(TARGET_SPEED, SPEED_GAIN * goal_distance) + acceleration = SPEED_GAIN * (target_speed - state.speed) + targets.append(target_index) + steering_history.append(steering) + state.update(acceleration, steering) + history.append([(step + 1) * DT, state.x, state.y, state.yaw, state.speed]) + raise RuntimeError("Vector Pursuit did not reach the goal within max_time") + + +def create_animation(course, history, targets, steering_history): + """Animate a completed simulation; the returned object can be saved as a GIF.""" + fig, (ax, steer_ax) = plt.subplots(2, 1, figsize=(9, 6), + gridspec_kw={"height_ratios": [3, 1]}) + ax.plot(course[:, 0], course[:, 1], "k--", label="Reference path") + ax.plot(*course[0, :2], "go", label="Path start") + ax.plot(*course[-1, :2], "r*", markersize=12, label="Goal") + track, = ax.plot([], [], "b-", label="Rear-axle trajectory") + body, = ax.plot([], [], "b-", linewidth=4, label="Vehicle heading") + target, = ax.plot([], [], "mo", label="Look-ahead target") + target_heading, = ax.plot([], [], "m-", linewidth=2) + ax.set(xlabel="x [m]", ylabel="y [m]", aspect="equal") + ax.legend(loc="upper left", ncol=3, fontsize=8) + ax.grid(True) + steering_line, = steer_ax.plot([], [], "b-") + steer_ax.axhline(math.degrees(MAX_STEER), color="r", linestyle="--") + steer_ax.axhline(-math.degrees(MAX_STEER), color="r", linestyle="--") + steer_ax.set(xlabel="Time [s]", ylabel="Steering [deg]", + xlim=(0.0, max(DT, history[-1, 0])), ylim=(-50.0, 50.0)) + steer_ax.grid(True) + fig.tight_layout() + + def update(index): + time, x, y, yaw, speed = history[index] + track.set_data(history[:index + 1, 1], history[:index + 1, 2]) + body.set_data([x, x + WHEELBASE * math.cos(yaw)], + [y, y + WHEELBASE * math.sin(yaw)]) + if len(targets): + tx, ty, target_yaw = course[targets[min(index, len(targets) - 1)]] + target.set_data([tx], [ty]) + target_heading.set_data([tx, tx + WHEELBASE * math.cos(target_yaw)], + [ty, ty + WHEELBASE * math.sin(target_yaw)]) + steering_line.set_data(history[:index, 0], np.rad2deg(steering_history[:index])) + ax.set_title(f"Vector Pursuit: t = {time:.1f} s, speed = {speed:.1f} m/s") + return track, body, target, target_heading, steering_line + + frames = list(range(0, len(history) - 1, 4)) + [len(history) - 1] + return FuncAnimation(fig, update, frames=frames, interval=1000 * 4 * DT, repeat=False) + + +def main(): + """Run the forward Vector Pursuit example, optionally displaying its animation.""" + result = simulate() + if show_animation: + animation = create_animation(*result) + plt.show() + return animation + return None + + +if __name__ == "__main__": + main() diff --git a/README.md b/README.md index 1a5c07f290..7f1f7f3d9c 100644 --- a/README.md +++ b/README.md @@ -51,6 +51,7 @@ Python codes and [textbook](https://atsushisakai.github.io/PythonRobotics/index. * [LQR based path planning](#lqr-based-path-planning) * [Optimal Trajectory in a Frenet Frame](#optimal-trajectory-in-a-frenet-frame) * [Path Tracking](#path-tracking) + * [Vector Pursuit](#vector-pursuit) * [move to a pose control](#move-to-a-pose-control) * [Stanley control](#stanley-control) * [Rear wheel feedback control](#rear-wheel-feedback-control) @@ -521,6 +522,16 @@ Reference # Path Tracking +## Vector Pursuit + +Forward bicycle-model path tracking using the position and tangent heading of +a look-ahead target, with steering limits and proportional speed control. + +![Vector Pursuit](https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/f379e41f9ba6aad4ada89139392e48325bfdc512/PathTracking/vector_pursuit/animation.gif) + +- [Algorithm documentation](https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/vector_pursuit/vector_pursuit.html) +- [Autonomous Ground Vehicle Path Tracking](https://doi.org/10.1002/rob.20031) + ## move to a pose control This is a simulation of moving to a pose control diff --git a/docs/modules/6_path_tracking/path_tracking_main.rst b/docs/modules/6_path_tracking/path_tracking_main.rst index 742cc807e6..1cfa1c9568 100644 --- a/docs/modules/6_path_tracking/path_tracking_main.rst +++ b/docs/modules/6_path_tracking/path_tracking_main.rst @@ -10,6 +10,7 @@ Path tracking is the ability of a robot to follow the reference path generated b :caption: Contents pure_pursuit_tracking/pure_pursuit_tracking + vector_pursuit/vector_pursuit stanley_control/stanley_control rear_wheel_feedback_control/rear_wheel_feedback_control lqr_steering_control/lqr_steering_control diff --git a/docs/modules/6_path_tracking/vector_pursuit/vector_pursuit_main.rst b/docs/modules/6_path_tracking/vector_pursuit/vector_pursuit_main.rst new file mode 100644 index 0000000000..b1a2a9d428 --- /dev/null +++ b/docs/modules/6_path_tracking/vector_pursuit/vector_pursuit_main.rst @@ -0,0 +1,86 @@ +Vector Pursuit +============== + +Vector Pursuit uses both the position and tangent heading of a look-ahead +point. It combines two instantaneous screw motions: travel along a circle +to the point, and correction of the remaining heading difference. + +.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/f379e41f9ba6aad4ada89139392e48325bfdc512/PathTracking/vector_pursuit/animation.gif + :alt: Bicycle model following an S-shaped path using Vector Pursuit + +Curvature from a target pose +---------------------------- + +Express the target in the rear-axle frame as :math:`(x_t,y_t,\theta)`, +where :math:`\theta` is the wrapped target-minus-vehicle heading. +This example uses positive counterclockwise angles and targets with +:math:`x_t>0`. + +The chord length, signed circular-arc angle, and arc length are + +.. math:: + + d=\sqrt{x_t^2+y_t^2},\qquad + \beta=2\operatorname{atan2}(y_t,x_t),\qquad + \ell=\frac{d}{\operatorname{sinc}(\beta/2)}, + +where :math:`\operatorname{sinc}(z)=\sin(z)/z` and +:math:`\operatorname{sinc}(0)=1`. +For forward speed :math:`v`, translation takes :math:`t_t=\ell/v`. +Setting rotation-correction time :math:`t_r=kt_t`, with :math:`k>0`, gives + +.. math:: + + \omega_t=\frac{v\beta}{\ell},\qquad + \omega_r=\frac{v(\theta-\beta)}{k\ell},\qquad + \kappa=\frac{\omega_t+\omega_r}{v} + =\frac{(k-1)\beta+\theta}{k\ell}. + +This is the curvature form of the paper's combined-screw construction. +It also handles :math:`y_t=0`, yielding :math:`\kappa=\theta/(kd)`. +A zero numerator means straight motion. Increasing :math:`k` reduces the +heading-correction contribution; the limit is pure-pursuit curvature +:math:`2y_t/d^2`. + +The bicycle steering command is + +.. math:: + + \delta=\operatorname{clip}\bigl(\arctan(L\kappa), + -\delta_{\max},\delta_{\max}\bigr). + +Here :math:`L` is the wheelbase. The kinematic model obeys +:math:`\dot x=v\cos\psi`, :math:`\dot y=v\sin\psi`, and +:math:`\dot\psi=v\tan\delta/L`. + +Example +------- + +The sample uses a cubic-spline path, a 4 m look-ahead along its sampled +arc length, :math:`k=2`, and proportional speed control. The blue line is +the rear-axle trajectory; blue and magenta segments indicate vehicle and +target headings. The lower panel shows steering and its limits. + +This forward-only example requires the target to stay ahead. It does not +support reverse driving, self-intersecting routes, obstacle avoidance, or +final-pose stabilization. Selecting the final target does not finish the +simulation: the rear axle must reach the goal tolerance. + +Code +---- + +.. autofunction:: PathTracking.vector_pursuit.vector_pursuit.main + +.. autofunction:: PathTracking.vector_pursuit.vector_pursuit.vector_pursuit_curvature + +.. autofunction:: PathTracking.vector_pursuit.vector_pursuit.simulate + +References +---------- + +- J. S. Wit, `Vector Pursuit Path Tracking for Autonomous Ground Vehicles + `__, PhD dissertation, + University of Florida, 2000. +- J. Wit, C. D. Crane III, D. Armstrong, `Autonomous Ground Vehicle Path Tracking + `__, Journal of Robotic Systems 21(8), + 439–449, 2004, equations 20–30. diff --git a/tests/test_vector_pursuit.py b/tests/test_vector_pursuit.py new file mode 100644 index 0000000000..43308f0fbe --- /dev/null +++ b/tests/test_vector_pursuit.py @@ -0,0 +1,141 @@ +import conftest # Add root path to sys.path +import math +import matplotlib.pyplot as plt +from matplotlib.animation import PillowWriter +import numpy as np +from numpy.testing import assert_allclose +import pytest + +from PathTracking.vector_pursuit import vector_pursuit as m + + +def test_straight_target_and_heading_correction_limit(): + assert m.vector_pursuit_curvature(4.0, 0.0, 0.0) == 0.0 + expected = 0.6 / (2.0 * 4.0) # Paper equation 30: omega / v = theta / (k*d). + for lateral_offset in (0.0, -1e-12, 1e-12): + assert_allclose(m.vector_pursuit_curvature(4.0, lateral_offset, 0.6), + expected, atol=1e-12) + + +@pytest.mark.parametrize("angle", [-1.2, -0.2, 0.2, 1.2]) +def test_target_tangent_to_circle_reproduces_circle_curvature(angle): + radius = 8.0 + target_x = radius * math.sin(abs(angle)) + target_y = math.copysign(radius * (1.0 - math.cos(angle)), angle) + for time_ratio in (0.5, 1.0, 2.0, 10.0): + curvature = m.vector_pursuit_curvature(target_x, target_y, angle, time_ratio) + assert_allclose(curvature, math.copysign(1.0 / radius, angle), atol=1e-14) + + +def test_heading_changes_steering_at_same_target_position(): + # Circle through (0,0) and (2,2) has radius 2 and translation rotation pi/2. + assert_allclose(m.vector_pursuit_curvature(2.0, 2.0, 0.0), 0.25) + assert_allclose(m.vector_pursuit_curvature(2.0, 2.0, math.pi / 2), 0.5) + # Opposing rotational screws cancel, giving a straight command, not infinity. + assert_allclose(m.vector_pursuit_curvature(2.0, 2.0, -math.pi / 2), 0.0) + + +def test_mirroring_and_heading_wrap(): + curvature = m.vector_pursuit_curvature(3.0, 1.0, 0.4) + assert_allclose(m.vector_pursuit_curvature(3.0, -1.0, -0.4), -curvature) + assert_allclose(m.vector_pursuit_curvature(3.0, 1.0, 0.4 + 2 * math.pi), curvature) + state = m.State(x=1.0, y=2.0, yaw=math.pi - 0.05) + target = np.array([-3.0, 2.2, -math.pi + 0.05]) + steering = m.steering_control(state, target) + target[2] += 2 * math.pi + assert_allclose(m.steering_control(state, target), steering) + + +def test_curvature_tends_to_pure_pursuit_when_rotation_correction_is_slow(): + assert_allclose(m.vector_pursuit_curvature(4.0, 1.0, -0.4, time_ratio=1e10), + 2.0 / 17.0, atol=1e-10) + + +@pytest.mark.parametrize("target_x, time_ratio", [(0.0, 2.0), (-1.0, 2.0), + (1.0, 0.0), (1.0, -1.0)]) +def test_rejects_nonforward_target_or_nonpositive_time_ratio(target_x, time_ratio): + with pytest.raises(ValueError): + m.vector_pursuit_curvature(target_x, 0.0, 0.0, time_ratio) + + +def test_steering_is_invariant_to_world_translation_and_rotation(): + state = m.State(x=1.0, y=-2.0, yaw=0.3) + target = np.array([6.0, 0.5, 0.8]) + angle = 1.2 + rotation = np.array([[math.cos(angle), -math.sin(angle)], + [math.sin(angle), math.cos(angle)]]) + translation = np.array([-3.0, 10.0]) + rotated_state = rotation @ [state.x, state.y] + translation + rotated_target = rotation @ target[:2] + translation + actual = m.steering_control(m.State(*rotated_state, yaw=state.yaw + angle), + [*rotated_target, target[2] + angle]) + assert_allclose(actual, m.steering_control(state, target), atol=1e-14) + + +def test_steering_limit_and_bicycle_straight_motion(): + state = m.State(y=0.0, speed=2.0) + assert m.steering_control(state, [0.1, 1.0, math.pi / 2]) == m.MAX_STEER + state.yaw = math.pi / 2 + state.update(0.0, 0.0) + assert_allclose([state.x, state.y, state.yaw], [0.0, 2.0 * m.DT, math.pi / 2], + atol=1e-14) + + +def test_target_selection_clamps_endpoint_and_preserves_progress(): + course = np.column_stack((np.arange(11.0), np.zeros((11, 2)))) + target, nearest = m.search_target_index(m.State(x=1.0, y=1.0), course) + assert (target, nearest) == (5, 1) + target, nearest = m.search_target_index(m.State(x=9.5, y=0.0), course, 8) + assert target == 10 and nearest >= 8 + assert m.search_target_index(m.State(x=10.0, y=0.0), course, 10) == (10, 10) + _, nearest = m.search_target_index(m.State(x=1.0, y=0.0), course, 8) + assert nearest == 8 + + +@pytest.mark.parametrize("initial_y, initial_yaw", [(-2.0, 0.0), (2.0, 0.2), (-3.0, -0.2)]) +def test_s_curve_reaches_goal_with_bounded_controls(initial_y, initial_yaw): + initial_state = m.State(y=initial_y, yaw=initial_yaw) + course, history, targets, steering = m.simulate(initial_state=initial_state) + assert np.isfinite(history).all() + assert np.linalg.norm(history[-1, 1:3] - course[-1, :2]) <= m.GOAL_TOLERANCE + assert np.max(abs(steering)) <= m.MAX_STEER + assert np.all(np.diff(targets) >= 0) + # Merely selecting the last target must not stop the vehicle early. + first_goal_target = np.flatnonzero(targets == len(course) - 1)[0] + assert np.linalg.norm(history[first_goal_target, 1:3] - course[-1, :2]) > 1.0 + assert (initial_state.y, initial_state.yaw, initial_state.speed) == (initial_y, initial_yaw, 0.0) + + +def test_straight_course_converges_from_lateral_offset(): + course = np.column_stack((np.arange(0.0, 30.1, 0.1), np.zeros((301, 2)))) + _, history, _, _ = m.simulate(course=course) + assert abs(history[-1, 2]) < 0.05 + assert abs(history[-1, 3]) < 0.02 + + +def test_timeout_is_reported_and_initial_goal_is_complete(tmp_path): + with pytest.raises(RuntimeError, match="did not reach"): + m.simulate(max_time=m.DT) + course = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]]) + result = m.simulate(course, m.State(x=1.0, y=0.0)) + _, history, targets, _ = result + assert len(history) == 1 and len(targets) == 0 + animation = m.create_animation(*result) + animation.save(tmp_path / "already_at_goal.gif", writer=PillowWriter(fps=5)) + plt.close("all") + + +def test_animation_and_headless_main(tmp_path, monkeypatch): + monkeypatch.setattr(m, "show_animation", False) + m.main() + course, history, targets, steering = m.simulate() + animation = m.create_animation(course, history[::len(history) - 1], + targets[:1], steering[:1]) + path = tmp_path / "vector_pursuit.gif" + animation.save(path, writer=PillowWriter(fps=5)) + assert path.stat().st_size > 0 + plt.close("all") + + +if __name__ == "__main__": + conftest.run_this_test(__file__)