fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 06:13:33 +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/lib/pure_pursuit/CMakeLists.txt
+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)
src/lib/pure_pursuit/PurePursuit.cpp
+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;
src/lib/pure_pursuit/PurePursuit.hpp
+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};
};
src/lib/pure_pursuit/PurePursuitTest.cpp
+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
}
src/lib/pure_pursuit/module.yaml
+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