fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

add pure pursuit library

Browse Source
This commit is contained in:
chfriedrich98 2024-07-09 12:21:00 +02:00 committed by Mathieu Bresciani
parent 08c790217d
commit a42dc2165c
5 changed files with 400 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/lib/CMakeLists.txt
View File

@ -64,6 +64,7 @@ add_subdirectory(perf EXCLUDE_FROM_ALL)
add_subdirectory(fw_performance_model EXCLUDE_FROM_ALL)
add_subdirectory(pid EXCLUDE_FROM_ALL)
add_subdirectory(pid_design EXCLUDE_FROM_ALL)
add_subdirectory(pure_pursuit EXCLUDE_FROM_ALL)
add_subdirectory(rate_control EXCLUDE_FROM_ALL)
add_subdirectory(rc EXCLUDE_FROM_ALL)
add_subdirectory(ringbuffer EXCLUDE_FROM_ALL)

39
src/lib/pure_pursuit/CMakeLists.txt Normal file
View File

@ -0,0 +1,39 @@
############################################################################
#
# Copyright (c) 2024 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(pure_pursuit
PurePursuit.cpp
PurePursuit.hpp
)
px4_add_unit_gtest(SRC PurePursuitTest.cpp LINKLIBS pure_pursuit)

75
src/lib/pure_pursuit/PurePursuit.cpp Normal file
View File

@ -0,0 +1,75 @@
/****************************************************************************
*
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "PurePursuit.hpp"
#include <mathlib/mathlib.h>
float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
const Vector2f &curr_pos_ned,
const float lookahead_distance)
{
// Check input validity
if (!curr_wp_ned.isAllFinite() || !curr_pos_ned.isAllFinite() || lookahead_distance < FLT_EPSILON
|| !PX4_ISFINITE(lookahead_distance) || !prev_wp_ned.isAllFinite()) {
return NAN;
}
// 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
|| 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));
}
const Vector2f prev_wp_to_curr_pos = curr_pos_ned - prev_wp_ned;
const Vector2f prev_wp_to_curr_wp_u = prev_wp_to_curr_wp.unit_or_zero();
const Vector2f distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) *
prev_wp_to_curr_wp_u; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp
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
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(),
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;
const Vector2f curr_pos_to_intersection_point = prev_wp_to_intersection_point - prev_wp_to_curr_pos;
return atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0));
}

91
src/lib/pure_pursuit/PurePursuit.hpp Normal file
View File

@ -0,0 +1,91 @@
/****************************************************************************
*
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <matrix/math.hpp>
using namespace matrix;
/**
* @file PurePursuit.hpp
*
* Implementation of pure pursuit guidance logic.
*
* Acknowledgements and References:
*
* This implementation has been built for PX4 based on the idea from [1] (not including any code).
*
* [1] Coulter, R. C. (1992). Implementation of the Pure Pursuit Path Tracking Algorithm
* (Techreport CMU-RI-TR-92-01).
*
* 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.
* C
* /
* __/__
* / / \
* / / \
* | / V |
* \/ /
* /\ _____ /
* N (0 rad) /
* ^ P
* |
* | D
* (-1.5708 rad) <----- ⨂ -----> E (1.5708 rad)
* |
* |
* ⌄
* (+- 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.
*/
class PurePursuit
{
public:
/**
* @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.
* Exceptions:
* Will return heading towards the current waypoint if it is closer to the vehicle than the lookahead.
* 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].
*/
float calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned,
const float lookahead_distance);
};

194
src/lib/pure_pursuit/PurePursuitTest.cpp Normal file
View File

@ -0,0 +1,194 @@
/****************************************************************************
*
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/******************************************************************
* Test code for the Pure Pursuit algorithm
* Run this test only using "make tests TESTFILTER=PurePursuit"
*
* Graphic interpretation:
* Legend:
* C: Current waypoint
* P: Previous waypoint
* V: Vehicle
* |: Line segment
* Orientation:
* C
* /
* __/__
* / / \
* / / \
* | / V |
* \/ /
* /\ _____ /
* N (0 rad) /
* ^ P
* |
* | D
* (-1.5708 rad) <----- ⨂ -----> E (1.5708 rad)
* |
* |
* ⌄
* (+- 3.14159 rad)
******************************************************************/
#include <gtest/gtest.h>
#include <lib/pure_pursuit/PurePursuit.hpp>
using namespace matrix;
TEST(PurePursuitTest, InvalidLookaheadDistance)
{
PurePursuit pure_pursuit;
// V C
// /
// /
// /
// P
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);
EXPECT_FALSE(PX4_ISFINITE(desired_heading1));
EXPECT_FALSE(PX4_ISFINITE(desired_heading2));
EXPECT_FALSE(PX4_ISFINITE(desired_heading3));
}
TEST(PurePursuitTest, InvalidWaypoints)
{
PurePursuit pure_pursuit;
// V C
// /
// /
// /
// P
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);
const float lookahead_distance{5.f};
// Prev WP is NAN
const float desired_heading1 = pure_pursuit.calcDesiredHeading(curr_wp_ned, Vector2f(NAN, NAN), curr_pos_ned,
lookahead_distance);
// Curr WP is NAN
const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(NAN, NAN), prev_wp_ned, curr_pos_ned,
lookahead_distance);
// Curr Pos is NAN
const float desired_heading3 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, Vector2f(NAN, NAN),
lookahead_distance);
EXPECT_FALSE(PX4_ISFINITE(desired_heading1));
EXPECT_FALSE(PX4_ISFINITE(desired_heading2));
EXPECT_FALSE(PX4_ISFINITE(desired_heading3));
}
TEST(PurePursuitTest, OutOfLookahead)
{
PurePursuit pure_pursuit;
const float lookahead_distance{5.f};
// V C
// /
// /
// /
// P
const float desired_heading1 = pure_pursuit.calcDesiredHeading(Vector2f(10.f, 10.f), Vector2f(0.f, 0.f), Vector2f(10.f,
0.f),
lookahead_distance);
// V
//
// P ----- C
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
}
TEST(PurePursuitTest, WaypointOverlap)
{
PurePursuit pure_pursuit;
const float lookahead_distance{5.f};
// C/P
//
//
//
// V
const float desired_heading1 = pure_pursuit.calcDesiredHeading(Vector2f(10.f, 10.f), Vector2f(10.f, 10.f), Vector2f(0.f,
0.f),
lookahead_distance);
// V
//
//
//
// C/P
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
}
TEST(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,
10.f),
lookahead_distance);
// V
// P ------ C
const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f),
Vector2f(5.f / sqrtf(2.f), 10.f),
lookahead_distance);
// V
// C ------ P
const float desired_heading3 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 0.f), Vector2f(0.f, 20.f),
Vector2f(5.f / sqrtf(2.f), 10.f),
lookahead_distance);
// V
//
// P ------ C
const float desired_heading4 = 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, 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
}