purePursuit: migrate parameters to library (#23438)

This commit is contained in:
chfriedrich98
2024-07-30 14:16:05 +02:00
committed by GitHub
parent 7b3d168af1
commit b93dd0e8d4
9 changed files with 168 additions and 110 deletions
@@ -15,9 +15,6 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge
param set-default NAV_ACC_RAD 0.5
param set-default RA_ACC_RAD_GAIN 2
param set-default RA_ACC_RAD_MAX 3
param set-default RA_LOOKAHD_GAIN 1
param set-default RA_LOOKAHD_MAX 10
param set-default RA_LOOKAHD_MIN 1
param set-default RA_MAX_ACCEL 1.5
param set-default RA_MAX_JERK 15
param set-default RA_MAX_SPEED 3
@@ -30,6 +27,11 @@ param set-default RA_SPEED_I 0.01
param set-default RA_SPEED_P 2
param set-default RA_WHEEL_BASE 0.321
# Pure Pursuit parameters
param set-default PP_LOOKAHD_GAIN 1
param set-default PP_LOOKAHD_MAX 10
param set-default PP_LOOKAHD_MIN 1
# Simulated sensors
param set-default SENS_EN_GPSSIM 1
param set-default SENS_EN_BAROSIM 0
+2 -1
View File
@@ -36,4 +36,5 @@ px4_add_library(pure_pursuit
PurePursuit.hpp
)
px4_add_unit_gtest(SRC PurePursuitTest.cpp LINKLIBS pure_pursuit)
px4_add_functional_gtest(SRC PurePursuitTest.cpp LINKLIBS pure_pursuit)
set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/module.yaml)
+28 -6
View File
@@ -34,21 +34,43 @@
#include "PurePursuit.hpp"
#include <mathlib/mathlib.h>
PurePursuit::PurePursuit(ModuleParams *parent) : ModuleParams(parent)
{
_param_handles.lookahead_gain = param_find("PP_LOOKAHD_GAIN");
_param_handles.lookahead_max = param_find("PP_LOOKAHD_MAX");
_param_handles.lookahead_min = param_find("PP_LOOKAHD_MIN");
updateParams();
}
void PurePursuit::updateParams()
{
param_get(_param_handles.lookahead_gain, &_params.lookahead_gain);
param_get(_param_handles.lookahead_max, &_params.lookahead_max);
param_get(_param_handles.lookahead_min, &_params.lookahead_min);
ModuleParams::updateParams();
}
float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
const Vector2f &curr_pos_ned,
const float lookahead_distance)
const float vehicle_speed)
{
// Check input validity
if (!curr_wp_ned.isAllFinite() || !curr_pos_ned.isAllFinite() || lookahead_distance < FLT_EPSILON
|| !PX4_ISFINITE(lookahead_distance) || !prev_wp_ned.isAllFinite()) {
if (!curr_wp_ned.isAllFinite() || !curr_pos_ned.isAllFinite() || vehicle_speed < -FLT_EPSILON
|| !PX4_ISFINITE(vehicle_speed) || !prev_wp_ned.isAllFinite()) {
return NAN;
}
_lookahead_distance = math::constrain(_params.lookahead_gain * vehicle_speed,
_params.lookahead_min, _params.lookahead_max);
// Pure pursuit
const Vector2f curr_pos_to_curr_wp = curr_wp_ned - curr_pos_ned;
const Vector2f prev_wp_to_curr_wp = curr_wp_ned - prev_wp_ned;
if (curr_pos_to_curr_wp.norm() < lookahead_distance
if (curr_pos_to_curr_wp.norm() < _lookahead_distance
|| prev_wp_to_curr_wp.norm() <
FLT_EPSILON) { // Target current waypoint if closer to it than lookahead or waypoints overlap
return atan2f(curr_pos_to_curr_wp(1), curr_pos_to_curr_wp(0));
@@ -61,11 +83,11 @@ float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2
const Vector2f curr_pos_to_path = distance_on_line_segment -
prev_wp_to_curr_pos; // Shortest vector from the current position to the path
if (curr_pos_to_path.norm() > lookahead_distance) { // Target closest point on path if there is no intersection point
if (curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point
return atan2f(curr_pos_to_path(1), curr_pos_to_path(0));
}
const float line_extension = sqrt(powf(lookahead_distance, 2.f) - powf(curr_pos_to_path.norm(),
const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(curr_pos_to_path.norm(),
2.f)); // Length of the vector from the endpoint of distance_on_line_segment to the intersection point
const Vector2f prev_wp_to_intersection_point = distance_on_line_segment + line_extension *
prev_wp_to_curr_wp_u;
+42 -7
View File
@@ -31,7 +31,10 @@
*
****************************************************************************/
#pragma once
#include <matrix/math.hpp>
#include <px4_platform_common/module_params.h>
using namespace matrix;
@@ -50,6 +53,10 @@ using namespace matrix;
* Pure pursuit is a path following algorithm that uses the intersection between the path and
* a circle (the radius of which is referred to as lookahead distance) around the vehicle as
* the target point for the vehicle.
* The lookahead distance is defined as v * k.
* v: Vehicle ground speed [m/s]
* k: Tuning parameter
* The lookahead distance is further constrained between an upper and lower threshhold.
* C
* /
* __/__
@@ -68,24 +75,52 @@ using namespace matrix;
* ⌄
* (+- 3.14159 rad)
*
* Input: Current/prev waypoint and the vehicle position in NED frame as well as the lookahead distance.
* Output: Calculates the intersection points and returns the heading towards the point that is closer to the current waypoint.
* Input: Current/prev waypoint and the vehicle position in NED frame as well as the vehicle speed.
* Output: Calculates the intersection points as described above and returns the heading towards the point that is closer to the current waypoint.
*/
class PurePursuit
class PurePursuit : public ModuleParams
{
public:
PurePursuit(ModuleParams *parent);
~PurePursuit() = default;
/**
* @brief Return heading towards the intersection point between a circle with a radius of
* lookahead_distance around the vehicle and an extended line segment from the previous to the current waypoint.
* vehicle_speed * PP_LOOKAHD_GAIN around the vehicle and an extended line segment from the previous to the current waypoint.
* Exceptions:
* Will return heading towards the current waypoint if it is closer to the vehicle than the lookahead.
* Will return heading towards the current waypoint if it is closer to the vehicle than the lookahead or if the waypoints overlap.
* Will return heading towards the closest point on the path if there are no intersection points (crosstrack error bigger than lookahead).
* Will return NAN if input is invalid.
* @param curr_wp_ned North/East coordinates of current waypoint in NED frame [m].
* @param prev_wp_ned North/East coordinates of previous waypoint in NED frame [m].
* @param curr_pos_ned North/East coordinates of current position of the vehicle in NED frame [m].
* @param lookahead_distance Radius of circle around vehicle [m].
* @param vehicle_speed Vehicle speed [m/s].
* @param RA_LOOKAHD_GAIN Tuning parameter [-]
* @param RA_LOOKAHD_MAX Maximum lookahead distance [m]
* @param RA_LOOKAHD_MIN Minimum lookahead distance [m]
*/
float calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned,
const float lookahead_distance);
float vehicle_speed);
float getLookaheadDistance() {return _lookahead_distance;};
protected:
/**
* @brief Update the parameters of the module.
*/
void updateParams() override;
struct {
param_t lookahead_gain;
param_t lookahead_max;
param_t lookahead_min;
} _param_handles{};
struct {
float lookahead_gain{1.f};
float lookahead_max{10.f};
float lookahead_min{1.f};
} _params{};
private:
float _lookahead_distance{0.f};
};
+29 -22
View File
@@ -59,6 +59,15 @@
* |
* ⌄
* (+- 3.14159 rad)
*
* NOTE:
* The tuning parameters for the pure pursuit algorithm are set to the following for all tests:
* PP_LOOKAHD_GAIN = 1.f
* PP_LOOKAHD_MAX = 10.f
* PP_LOOKAHD_MIN = 1.f
* This way passing the vehicle_speed in calcDesiredHeading function is equivalent to passing
* the lookahead distance.
*
******************************************************************/
#include <gtest/gtest.h>
@@ -66,9 +75,14 @@
using namespace matrix;
TEST(PurePursuitTest, InvalidLookaheadDistance)
class PurePursuitTest : public ::testing::Test
{
public:
PurePursuit pure_pursuit{nullptr};
};
TEST_F(PurePursuitTest, InvalidSpeed)
{
PurePursuit pure_pursuit;
// V C
// /
// /
@@ -77,20 +91,16 @@ TEST(PurePursuitTest, InvalidLookaheadDistance)
const Vector2f curr_wp_ned(10.f, 10.f);
const Vector2f prev_wp_ned(0.f, 0.f);
const Vector2f curr_pos_ned(10.f, 0.f);
// Zero lookahead
const float desired_heading1 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, 0.f);
// Negative lookahead
const float desired_heading2 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, -1.f);
// NaN lookahead
const float desired_heading3 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, NAN);
// Negative speed
const float desired_heading1 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, -1.f);
// NaN speed
const float desired_heading2 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, NAN);
EXPECT_FALSE(PX4_ISFINITE(desired_heading1));
EXPECT_FALSE(PX4_ISFINITE(desired_heading2));
EXPECT_FALSE(PX4_ISFINITE(desired_heading3));
}
TEST(PurePursuitTest, InvalidWaypoints)
TEST_F(PurePursuitTest, InvalidWaypoints)
{
PurePursuit pure_pursuit;
// V C
// /
// /
@@ -115,9 +125,8 @@ TEST(PurePursuitTest, InvalidWaypoints)
EXPECT_FALSE(PX4_ISFINITE(desired_heading3));
}
TEST(PurePursuitTest, OutOfLookahead)
TEST_F(PurePursuitTest, OutOfLookahead)
{
PurePursuit pure_pursuit;
const float lookahead_distance{5.f};
// V C
// /
@@ -133,13 +142,12 @@ TEST(PurePursuitTest, OutOfLookahead)
const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f,
10.f),
lookahead_distance);
EXPECT_NEAR(desired_heading1, M_PI_2_F + M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to current waypoint
EXPECT_NEAR(desired_heading2, M_PI_F, FLT_EPSILON); // Fallback: Bearing to current waypoint
EXPECT_NEAR(desired_heading1, M_PI_2_F + M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to closest point on path
EXPECT_NEAR(desired_heading2, M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path
}
TEST(PurePursuitTest, WaypointOverlap)
TEST_F(PurePursuitTest, WaypointOverlap)
{
PurePursuit pure_pursuit;
const float lookahead_distance{5.f};
// C/P
//
@@ -157,13 +165,12 @@ TEST(PurePursuitTest, WaypointOverlap)
const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 0.f), Vector2f(0.f, 0.f), Vector2f(10.f,
10.f),
lookahead_distance);
EXPECT_NEAR(desired_heading1, M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to current waypoint
EXPECT_NEAR(desired_heading2, -(M_PI_4_F + M_PI_2_F), FLT_EPSILON); // Fallback: Bearing to current waypoint
EXPECT_NEAR(desired_heading1, M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to closest point on path
EXPECT_NEAR(desired_heading2, -(M_PI_4_F + M_PI_2_F), FLT_EPSILON); // Fallback: Bearing to closest point on path
}
TEST(PurePursuitTest, CurrAndPrevSameNorthCoordinate)
TEST_F(PurePursuitTest, CurrAndPrevSameNorthCoordinate)
{
PurePursuit pure_pursuit;
const float lookahead_distance{5.f};
// P -- V -- C
const float desired_heading1 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(0.f,
@@ -190,5 +197,5 @@ TEST(PurePursuitTest, CurrAndPrevSameNorthCoordinate)
EXPECT_NEAR(desired_heading1, M_PI_2_F, FLT_EPSILON);
EXPECT_NEAR(desired_heading2, M_PI_2_F + M_PI_4_F, FLT_EPSILON);
EXPECT_NEAR(desired_heading3, -(M_PI_2_F + M_PI_4_F), FLT_EPSILON);
EXPECT_NEAR(desired_heading4, M_PI_F, FLT_EPSILON); // Fallback: Bearing to current waypoint
EXPECT_NEAR(desired_heading4, M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path
}
+37
View File
@@ -0,0 +1,37 @@
module_name: Pure Pursuit
parameters:
- group: Pure Pursuit
definitions:
PP_LOOKAHD_GAIN:
description:
short: Tuning parameter for the pure pursuit controller
long: Lower value -> More aggressive controller (beware overshoot/oscillations)
type: float
min: 0.1
max: 100
increment: 0.01
decimal: 2
default: 1
PP_LOOKAHD_MIN:
description:
short: Minimum lookahead distance for the pure pursuit controller
type: float
unit: m
min: 0.1
max: 100
increment: 0.01
decimal: 2
default: 1
PP_LOOKAHD_MAX:
description:
short: Maximum lookahead distance for the pure pursuit controller
type: float
unit: m
min: 0.1
max: 100
increment: 0.01
decimal: 2
default: 10
@@ -60,7 +60,6 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
{
// Initializations
float desired_speed{0.f};
float desired_steering{0.f};
float vehicle_yaw{0.f};
float actual_speed{0.f};
bool mission_finished{false};
@@ -116,8 +115,10 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
}
// Guidance logic
if (!mission_finished) {
// Calculate desired speed
if (mission_finished) {
_desired_steering = 0.f;
} else {
const float distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
_prev_wp(0),
_prev_wp(1));
@@ -125,7 +126,11 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
_curr_wp(0),
_curr_wp(1));
if (distance_to_curr_wp > _acceptance_radius) {
if (distance_to_curr_wp < _acceptance_radius) { // Catch delay command
desired_speed = 0.f;
} else {
// Calculate desired speed
if (_param_ra_miss_vel_min.get() > 0.f && _param_ra_miss_vel_min.get() < _param_ra_miss_vel_def.get()
&& _param_ra_miss_vel_gain.get() > FLT_EPSILON) { // Cornering slow down effect
if (distance_to_prev_wp <= _prev_acceptance_radius && _prev_acceptance_radius > FLT_EPSILON) {
@@ -163,15 +168,10 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
}
// Calculate desired steering
desired_steering = calcDesiredSteering(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, _param_ra_lookahd_gain.get(),
_param_ra_lookahd_min.get(), _param_ra_lookahd_max.get(), _param_ra_wheel_base.get(), desired_speed, vehicle_yaw);
desired_steering = math::constrain(desired_steering, -_param_ra_max_steer_angle.get(),
_param_ra_max_steer_angle.get());
_prev_desired_steering = desired_steering;
} else { // Catch delay command
desired_steering = _prev_desired_steering; // Avoid steering on the spot
desired_speed = 0.f;
_desired_steering = calcDesiredSteering(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, _param_ra_wheel_base.get(),
desired_speed, vehicle_yaw);
_desired_steering = math::constrain(_desired_steering, -_param_ra_max_steer_angle.get(),
_param_ra_max_steer_angle.get());
}
}
@@ -203,7 +203,7 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
// Return motor setpoints
motor_setpoint motor_setpoint_temp;
motor_setpoint_temp.steering = math::interpolate<float>(desired_steering, -_param_ra_max_steer_angle.get(),
motor_setpoint_temp.steering = math::interpolate<float>(_desired_steering, -_param_ra_max_steer_angle.get(),
_param_ra_max_steer_angle.get(),
-1.f, 1.f);
motor_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f);
@@ -258,8 +258,8 @@ void RoverAckermannGuidance::updateWaypoints()
}
float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
const Vector2f &next_wp_ned, const float &default_acceptance_radius, const float &acceptance_radius_gain,
const float &acceptance_radius_max, const float &wheel_base, const float &max_steer_angle)
const Vector2f &next_wp_ned, const float default_acceptance_radius, const float acceptance_radius_gain,
const float acceptance_radius_max, const float wheel_base, const float max_steer_angle)
{
// Setup variables
const Vector2f curr_to_prev_wp_ned = prev_wp_ned - curr_wp_ned;
@@ -287,14 +287,12 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned
}
float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
const Vector2f &curr_pos_ned, const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max,
const float &wheel_base, const float &desired_speed, const float &vehicle_yaw)
const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, const float vehicle_yaw)
{
// Calculate desired steering to reach lookahead point
const float lookahead_distance = math::constrain(lookahead_gain * desired_speed,
lookahead_min, lookahead_max);
const float desired_heading = _pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned,
lookahead_distance);
desired_speed);
const float lookahead_distance = _pure_pursuit.getLookaheadDistance();
const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw);
// For logging
_rover_ackermann_guidance_status.lookahead_distance = lookahead_distance;
@@ -82,7 +82,7 @@ public:
* @brief Compute guidance for ackermann rover and return motor_setpoint for throttle and steering.
* @param nav_state Vehicle navigation state
*/
motor_setpoint computeGuidance(const int nav_state);
motor_setpoint computeGuidance(int nav_state);
/**
* @brief Update global/NED waypoint coordinates and acceptance radius
@@ -103,24 +103,20 @@ public:
* @param max_steer_angle Rover maximum steer angle.
*/
float updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
const Vector2f &next_wp_ned, const float &default_acceptance_radius, const float &acceptance_radius_gain,
const float &acceptance_radius_max, const float &wheel_base, const float &max_steer_angle);
const Vector2f &next_wp_ned, float default_acceptance_radius, float acceptance_radius_gain,
float acceptance_radius_max, float wheel_base, float max_steer_angle);
/**
* @brief Calculate and return desired steering input
* @param curr_wp_ned Current waypoint in NED frame.
* @param prev_wp_ned Previous waypoint in NED frame.
* @param curr_pos_ned Current position of the vehicle in NED frame.
* @param lookahead_gain Tuning parameter for the lookahead distance pure pursuit controller.
* @param lookahead_min Minimum lookahead distance.
* @param lookahead_max Maximum lookahead distance.
* @param wheel_base Rover wheelbase.
* @param desired_speed Desired speed for the rover.
* @param vehicle_yaw Current yaw of the rover.
*/
float calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned,
const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max, const float &wheel_base,
const float &desired_speed, const float &vehicle_yaw);
float wheel_base, float desired_speed, float vehicle_yaw);
protected:
/**
@@ -144,14 +140,14 @@ private:
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates.
PurePursuit _pure_pursuit; // Pure pursuit library
PurePursuit _pure_pursuit{this}; // Pure pursuit library
// Rover variables
Vector2d _curr_pos{};
Vector2f _curr_pos_ned{};
PID_t _pid_throttle;
hrt_abstime _timestamp{0};
float _prev_desired_steering{0.f};
float _desired_steering{0.f};
// Waypoint variables
Vector2d _curr_wp{};
@@ -168,9 +164,6 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_steer_angle,
(ParamFloat<px4::params::RA_LOOKAHD_GAIN>) _param_ra_lookahd_gain,
(ParamFloat<px4::params::RA_LOOKAHD_MAX>) _param_ra_lookahd_max,
(ParamFloat<px4::params::RA_LOOKAHD_MIN>) _param_ra_lookahd_min,
(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain,
(ParamFloat<px4::params::RA_MISS_VEL_DEF>) _param_ra_miss_vel_def,
-37
View File
@@ -27,43 +27,6 @@ parameters:
decimal: 2
default: 0.5236
RA_LOOKAHD_GAIN:
description:
short: Tuning parameter for the pure pursuit controller
long: Lower value -> More aggressive controller (beware overshoot/oscillations)
type: float
min: 0.1
max: 100
increment: 0.01
decimal: 2
default: 1
RA_LOOKAHD_MAX:
description:
short: Maximum lookahead distance for the pure pursuit controller
long: |
This is the maximum crosstrack error before the controller starts
targeting the current waypoint rather then the path between the previous
and next waypoint.
type: float
unit: m
min: 0.1
max: 100
increment: 0.01
decimal: 2
default: 10
RA_LOOKAHD_MIN:
description:
short: Minimum lookahead distance for the pure pursuit controller
type: float
unit: m
min: 0.1
max: 100
increment: 0.01
decimal: 2
default: 1
RA_ACC_RAD_MAX:
description:
short: Maximum acceptance radius for the waypoints