From 61aa919aac3bb02dc1344299f4154f7c723570bf Mon Sep 17 00:00:00 2001 From: Kostubh Khandelwal Date: Thu, 26 Sep 2024 12:43:44 +0000 Subject: [PATCH] Added Vector Pursuit Controller Signed-off-by: Kostubh Khandelwal --- configuration/index.rst | 1 + .../configuring-vector-pursuit-controller.rst | 394 ++++++++++++++++++ plugins/index.rst | 61 +-- setup_guides/algorithm/select_algorithm.rst | 4 + tuning/index.rst | 4 + 5 files changed, 436 insertions(+), 28 deletions(-) create mode 100644 configuration/packages/configuring-vector-pursuit-controller.rst diff --git a/configuration/index.rst b/configuration/index.rst index aaff8e8ad..ba9f1767e 100644 --- a/configuration/index.rst +++ b/configuration/index.rst @@ -21,6 +21,7 @@ the best navigation performance. packages/configuring-controller-server.rst packages/configuring-dwb-controller.rst packages/configuring-regulated-pp.rst + packages/configuring-vector-pursuit-controller.rst packages/configuring-mppic.rst packages/configuring-rotation-shim-controller.rst packages/configuring-graceful-motion-controller.rst diff --git a/configuration/packages/configuring-vector-pursuit-controller.rst b/configuration/packages/configuring-vector-pursuit-controller.rst new file mode 100644 index 000000000..f0bd7905b --- /dev/null +++ b/configuration/packages/configuring-vector-pursuit-controller.rst @@ -0,0 +1,394 @@ +.. _configuring_vector_pursuit_controller: + +Vector Pursuit Controller +######################### + +Source code on Github_. + +.. _Github: https://github.com/blackcoffeerobotics/vector_pursuit_controller + +The Vector Pursuit controller implements a path-tracking algorithm based on the theory of screws. Unlike most other controllers, Vector Pursuit incorporates the path's orientation with the position when calculating velocity commands. +The use of screws unifies rotation and translation, making the controller more robust and geometrically meaningful, especially in complex, dynamic environments. This enables the robot to follow the path more smoothly and accurately, especially when navigating tight corners or moving at high speeds. +By adjusting linear velocities based on path curvature, the controller minimizes overshoot in sharp turns, enhancing safety and stability. + +See the package's ``README`` for more complete information. + + +Vector Pursuit Parameters +********************************* + +:k: + + ============== =========================== + Type Default + -------------- --------------------------- + double 8.0 + ============== =========================== + + Description + k is a constant that relates the rotation and translation times when calculating the resultant screw(as a sum of the rotation and translation screws). The relationship is rotation_time = k * translation_time. Increasing k will put more weightage on faster translation and decreasing k will, in turn, apply more weightage on faster rotation. + +:desired_linear_vel: + + ============== =========================== + Type Default + -------------- --------------------------- + double 0.5 + ============== =========================== + + Description + The desired maximum linear velocity (m/s). +:min_turning_radius: + + ============== =========================== + Type Default + -------------- --------------------------- + double 1.0 + ============== =========================== + + Description + The minimum achievable turning radius(m) of the robot. Remember, sharper turns have smaller radii. + +:lookahead_dist: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.6 + ============== ============================= + + Description + The lookahead distance (m) to find the lookahead point. + +:min_lookahead_dist: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.3 + ============== ============================= + + Description + The minimum lookahead distance (m) threshold for adaptive lookahead point. + +:max_lookahead_dist: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.9 + ============== ============================= + + Description + The maximum lookahead distance (m) threshold for adaptive lookahead point. + +:lookahead_time: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 1.5 + ============== ============================= + + Description + The time (s) to integrate the current linear velocity to get the scaled lookahead distance for adaptive lookahead point. + +:rotate_to_heading_angular_vel: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 1.8 + ============== ============================= + + Description + Angular velocity for rotating to heading. + +:transform_tolerance: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.1 + ============== ============================= + + Description + The TF transform tolerance (s). + +:use_velocity_scaled_lookahead_dist: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool false + ============== ============================= + + Description + Enable velocity adaptive ``lookahead_distance``. + +:min_approach_linear_velocity: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.05 + ============== ============================= + + Description + The minimum velocity approach aware linear velocity scaling can produce. + +:approach_velocity_scaling_dist: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 1.0 + ============== ============================= + + Description + The distance to goal at which velocity scaling will begin. Set to 0 to disable. + +:use_collision_detection: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool true + ============== ============================= + + Description + Enable/disable collision detection. + +:max_allowed_time_to_collision_up_to_target: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 1.0 + ============== ============================= + + Description + Maximum time (s) allowed for collision checking. + +:use_cost_regulated_linear_velocity_scaling: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool true + ============== ============================= + + Description + Whether to slowdown in close proximity to obstacles. + +:cost_scaling_dist: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.6 + ============== ============================= + + Description + Distance for cost-based velocity scaling. + +:cost_scaling_gain: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 1.0 + ============== ============================= + + Description + Multiplicative factor for cost-based velocity scaling. + +:inflation_cost_scaling_factor: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 3.0 + ============== ============================= + + Description + Factor for inflation cost scaling. + +:min_linear_velocity: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.05 + ============== ============================= + + Description + The minimum speed (m/s) robot must run at. Must be ``> 0.01``. + +:use_rotate_to_heading: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool true + ============== ============================= + + Description + Enable/disable rotate-to-heading behavior. Will override reversing if both are enabled. + +:allow_reversing: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool false + ============== ============================= + + Description + Enable/Disable reversing movement. Will move in reverse if the lookahead point is behind the robot. + +:rotate_to_heading_min_angle: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.785 + ============== ============================= + + Description + The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place, if ``use_rotate_to_heading`` is ``true``. + +:max_angular_accel: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 3.2 + ============== ============================= + + Description + Maximum allowable angular acceleration (rad/s^2). + +:max_linear_accel: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 2.0 + ============== ============================= + + Description + Maximum allowable linear acceleration (m/s^2). + +:max_lateral_accel: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.5 + ============== ============================= + + Description + Maximum allowable acceleration (m/s^2) perpendicular to axis of desired translation motion. This limit is used to restrict the maximum speed for a given path curvature. + +:use_interpolation: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool true + ============== ============================= + + Description + Calculate lookahead point exactly at the lookahead distance. Otherwise select a discrete point on the path. + +:use_heading_from_path: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool false + ============== ============================= + + Description + If set to true, uses the orientation from the path poses otherwise compute appropriate orientations. Only set to true if using a planner that takes robot heading into account like Smac Planner. + +:max_robot_pose_search_dist: + + ============== ================================================= + Type Default + -------------- ------------------------------------------------- + double Local costmap max extent (max(width, height) / 2) + ============== ================================================= + + Description + Maximum search distance for target poses along the global plan. + +Example +******* +.. code-block:: yaml + + controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.25 + movement_time_allowance: 10.0 + + # Goal checker parameters + general_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + + FollowPath: + plugin: "vector_pursuit_controller::VectorPursuitController" + k: 5.0 + desired_linear_vel: 0.5 + min_turning_radius: 0.25 + lookahead_dist: 1.0 + min_lookahead_dist: 0.5 + max_lookahead_dist: 1.5 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 0.5 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: false + min_linear_velocity: 0.0 + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.5 + max_allowed_time_to_collision_up_to_target: 1.0 + use_collision_detection: true + use_cost_regulated_linear_velocity_scaling: true + cost_scaling_dist: 0.5 + cost_scaling_gain: 1.0 + inflation_cost_scaling_factor: 3.0 + use_rotate_to_heading: true + allow_reversing: false + rotate_to_heading_min_angle: 0.5 + max_angular_accel: 3.0 + max_linear_accel: 2.0 + max_lateral_accel: 0.2 + max_robot_pose_search_dist: 10.0 + use_interpolation: true + use_heading_from_path: false + approach_velocity_scaling_dist: 1.0 + +Acknowledgements +**************** + +We acknowledge the contributions of: + +1. The author of `Vector Pursuit Path Tracking for Autonomous Ground Vehicles `_, Jeffrey S. Wit. +2. The `Nav2 Regulated Pure Pursuit Controller project `_. diff --git a/plugins/index.rst b/plugins/index.rst index 4c44dd462..9087b8267 100644 --- a/plugins/index.rst +++ b/plugins/index.rst @@ -103,34 +103,39 @@ Costmap Filters Controllers =========== -+--------------------------------+--------------------+----------------------------------+-----------------------+ -| Plugin Name | Creator | Description | Drivetrain support | -+================================+====================+==================================+=======================+ -| `DWB Controller`_ | David Lu!! | A highly configurable DWA | Differential, | -| | | implementation with plugin | Omnidirectional, | -| | | interfaces | Legged | -+--------------------------------+--------------------+----------------------------------+-----------------------+ -| `TEB Controller`_ | Christoph Rösmann | A MPC-like controller suitable | **Ackermann**, Legged,| -| | | for ackermann, differential, and | Omnidirectional, | -| | | holonomic robots. | Differential | -+--------------------------------+--------------------+----------------------------------+-----------------------+ -| `Regulated Pure Pursuit`_ | Steve Macenski | A service / industrial robot | **Ackermann**, Legged,| -| | | variation on the pure pursuit | Differential | -| | | algorithm with adaptive features.| | -+--------------------------------+--------------------+----------------------------------+-----------------------+ -| `MPPI Controller`_ | Steve Macenski | A predictive MPC controller with | Differential, Omni, | -| | Aleksei Budyakov | modular & custom cost functions | **Ackermann** | -| | | that can accomplish many tasks. | | -+--------------------------------+--------------------+----------------------------------+-----------------------+ -| `Rotation Shim Controller`_ | Steve Macenski | A "shim" controller to rotate | Differential, Omni, | -| | | to path heading before passing | model rotate in place | -| | | to main controller for tracking.| | -+--------------------------------+--------------------+----------------------------------+-----------------------+ -| `Graceful Controller`_ | Alberto Tudela | A controller based on a | Differential | -| | | pose-following control law to | | -| | | generate smooth trajectories. | | -+--------------------------------+--------------------+----------------------------------+-----------------------+ - ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| Plugin Name | Creator | Description | Drivetrain support | ++================================+======================+==================================+=======================+ +| `Vector Pursuit Controller`_ | Black Coffee Robotics| A controller based on the vector | Differential, | +| | | pursuit algorithm useful for | Ackermann, Legged, | +| | | high speed accurate path tracking| | ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| `DWB Controller`_ | David Lu!! | A highly configurable DWA | Differential, | +| | | implementation with plugin | Omnidirectional, | +| | | interfaces | Legged | ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| `TEB Controller`_ | Christoph Rösmann | A MPC-like controller suitable | **Ackermann**, Legged,| +| | | for ackermann, differential, and | Omnidirectional, | +| | | holonomic robots. | Differential | ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| `Regulated Pure Pursuit`_ | Steve Macenski | A service / industrial robot | **Ackermann**, Legged,| +| | | variation on the pure pursuit | Differential | +| | | algorithm with adaptive features.| | ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| `MPPI Controller`_ | Steve Macenski | A predictive MPC controller with | Differential, Omni, | +| | Aleksei Budyakov | modular & custom cost functions | **Ackermann** | +| | | that can accomplish many tasks. | | ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| `Rotation Shim Controller`_ | Steve Macenski | A "shim" controller to rotate | Differential, Omni, | +| | | to path heading before passing | model rotate in place | +| | | to main controller for tracking.| | ++--------------------------------+----------------------+----------------------------------+-----------------------+ +| `Graceful Controller`_ | Alberto Tudela | A controller based on a | Differential | +| | | pose-following control law to | | +| | | generate smooth trajectories. | | ++--------------------------------+----------------------+----------------------------------+-----------------------+ + +.. _Vector Pursuit Controller: https://github.com/blackcoffeerobotics/vector_pursuit_controller .. _DWB Controller: https://github.com/ros-planning/navigation2/tree/main/nav2_dwb_controller .. _TEB Controller: https://github.com/rst-tu-dortmund/teb_local_planner .. _Regulated Pure Pursuit: https://github.com/ros-planning/navigation2/tree/main/nav2_regulated_pure_pursuit_controller diff --git a/setup_guides/algorithm/select_algorithm.rst b/setup_guides/algorithm/select_algorithm.rst index 808947cb2..80979cf40 100644 --- a/setup_guides/algorithm/select_algorithm.rst +++ b/setup_guides/algorithm/select_algorithm.rst @@ -77,6 +77,8 @@ The default controller plugin is the `DWB controller `_ . It implements the `Vector Pursuit algorithm `_ and calculates the command velocity using screw theory. This controller is suitable for high speed path tracking and sharp turns or when computation resources are limited. It can be used for **differential, ackermann, and legged** robots. + Another example of a controller server plugin is the `TEB controller `_ which is an MPC time optimal controller. It implements the Timed Elastic Band (TEB) approach which optimizes the robot's trajectory based on its execution time, distance from obstacles, and feasibility with respect to the robot's kinematic constraints. This controller can be used on **differential, omnidirectional, ackermann, and legged** robots. The last example for this section is the `Regulated Pure Pursuit controller (RPP) `_ . This controller implements a variant of the pure pursuit algorithm with added regulation heuristic functions to manage collision and velocity constraints. This variation is implemented to target the needs of service or industrial robots and is suitable for use with **differential, ackermann, and legged** robots. @@ -87,6 +89,8 @@ Summary +-----------------+---------------------------------------------------+----------------------------+ | Plugin Name | Supported Robot Types | Task | +=================+===================================================+============================+ +| VP controller | Differential, Ackermann, Legged | High speed path tracking | ++-----------------+---------------------------------------------------+----------------------------+ | DWB controller | Differential, Omnidirectional | Dynamic obstacle avoidance | +-----------------+---------------------------------------------------+ | | TEB Controller | Differential, Omnidirectional, Ackermann, Legged | | diff --git a/tuning/index.rst b/tuning/index.rst index 77e10b562..a55906eeb 100644 --- a/tuning/index.rst +++ b/tuning/index.rst @@ -86,6 +86,8 @@ In general though, the following table is a good first-order description of the +----------------+---------------------------------------------------+----------------------------+ | Plugin Name | Supported Robot Types | Task | +================+===================================================+============================+ +| VP controller | Differential, Ackermann, Legged | High speed path tracking | ++----------------+---------------------------------------------------+----------------------------+ | DWB controller | Differential, Omnidirectional | Dynamic obstacle avoidance | +----------------+---------------------------------------------------+ | | MPPI Controller| Differential, Omnidirectional, Ackermann, Legged | Dynamic obstacle avoidance | @@ -99,6 +101,8 @@ All of the above controllers can handle both circular and arbitrary shaped robot Regulated Pure Pursuit is good for exact path following and is typically paired with one of the kinematically feasible planners (eg State Lattice, Hybrid-A\*, etc) since those paths are known to be drivable given hard physical constraints. However, it can also be applied to differential drive robots who can easily pivot to match any holonomic path. This is the plugin of choice if you simply want your robot to follow the path, rather exactly, without any dynamic obstacle avoidance or deviation. It is simple and geometric, as well as slowing the robot in the presence of near-by obstacles *and* while making sharp turns. +Vector Pursuit is another good path tracking solution and just like RPP, is paired with a kinematically feasible planner. It is a bit more advanced than RPP in the sense it also takes path heading into account and can handle more complex paths at higher speeds, but it is still a simple geometric controller thus requiring low computation resources. + DWB and MPPI are both options that will track paths, but also diverge from the path if there are dynamic obstacles present (in order to avoid them). DWB does this through scoring multiple trajectories on a set of critics. These trajectories are also generated via plugins that can be replaced, but support out of the box Omni and Diff robot types within the valid velocity and acceleration restrictions. These critics are plugins that can be selected at run-time and contain weights that may be tuned to create the desired behavior, such as minimizing path distance, minimizing distance to the goal or headings, and other action penalties that can be designed. This does require a bit of tuning for a given platform, application, and desired behavior, but it is possible to tune DWB to do nearly any single thing well. MPPI on the other hand implements an optimization based approach, using randomly perturbed samples of the previous optimal trajectory to maximize a set of plugin-based objective functions. In that regard, it is similar to DWB however MPPI is a far more modern and advanced technique that will deal with dynamic agents in the environment and create intelligent behavior due to the optimization based trajectory planning, rather then DWB's constant action model. MPPI however does have moderately higher compute costs, but it is highly recommended to go this route and has received considerable development resources and attention due to its power. This typically works pretty well out of the box, but to tune for specific behaviors, you may have to retune some of the parameters. The README.md file for this package contains details on how to tune it efficiently.