fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 23:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

purePursuit: migrate parameters to library (#23438)

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp
+19 -21
View File
@@ -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;
src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp
+6 -13
View File
@@ -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,
src/modules/rover_ackermann/module.yaml
-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