Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-18 08:04:58 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

ackermann: implement pure pursuit lib

Browse Source
This commit is contained in:
chfriedrich98
2024-07-18 13:34:56 +02:00
committed by Mathieu Bresciani
parent f2bca92221
commit f8bebd9e41
4 changed files with 14 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/RoverAckermannGuidanceStatus.msg
-1
View File
@@ -4,6 +4,5 @@ float32 desired_speed # [m/s] Rover desired ground speed
float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller
float32 heading_error # [deg] Heading error of the pure pursuit controller
float32 pid_throttle_integral # [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions
float32 crosstrack_error # [m] Shortest distance from the vehicle to the path
# TOPICS rover_ackermann_guidance_status
src/modules/rover_ackermann/CMakeLists.txt
+1
View File
@@ -43,6 +43,7 @@ px4_add_module(
RoverAckermannGuidance
px4_work_queue
SlewRate
pure_pursuit
MODULE_CONFIG
module.yaml
)
src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp
+11 -79
View File
@@ -209,21 +209,24 @@ void RoverAckermannGuidance::updateWaypoints()
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
// Global waypoint coordinates
if (position_setpoint_triplet.current.valid) {
if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
&& PX4_ISFINITE(position_setpoint_triplet.current.lon)) {
_curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon);
} else {
_curr_wp = Vector2d(0, 0);
}
if (position_setpoint_triplet.previous.valid) {
if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat)
&& PX4_ISFINITE(position_setpoint_triplet.previous.lon)) {
_prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon);
} else {
_prev_wp = _curr_pos;
}
if (position_setpoint_triplet.next.valid) {
if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat)
&& PX4_ISFINITE(position_setpoint_triplet.next.lon)) {
_next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
} else {
@@ -280,45 +283,16 @@ float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_local,
const Vector2f &curr_pos_local, const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max,
const float &wheel_base, const float &desired_speed, const float &vehicle_yaw)
{
// Calculate crosstrack error
const Vector2f prev_wp_to_curr_wp_local = curr_wp_local - prev_wp_local;
if (prev_wp_to_curr_wp_local.norm() < FLT_EPSILON) { // Avoid division by 0 (this case should not happen)
return 0.f;
}
const Vector2f prev_wp_to_curr_pos_local = curr_pos_local - prev_wp_local;
const Vector2f distance_on_line_segment = ((prev_wp_to_curr_pos_local * prev_wp_to_curr_wp_local) /
prev_wp_to_curr_wp_local.norm()) * prev_wp_to_curr_wp_local.normalized();
const Vector2f crosstrack_error = (prev_wp_local + distance_on_line_segment) - curr_pos_local;
// Calculate desired heading towards lookahead point
float desired_heading{0.f};
float lookahead_distance = math::constrain(lookahead_gain * desired_speed,
lookahead_min, lookahead_max);
if (crosstrack_error.longerThan(lookahead_distance)) {
if (crosstrack_error.norm() < lookahead_max) {
lookahead_distance = crosstrack_error.norm(); // Scale lookahead radius
desired_heading = calcDesiredHeading(curr_wp_local, prev_wp_local, curr_pos_local, lookahead_distance);
} else { // Excessively large crosstrack error
desired_heading = calcDesiredHeading(curr_wp_local, curr_pos_local, curr_pos_local, lookahead_distance);
}
} else { // Crosstrack error smaller than lookahead
desired_heading = calcDesiredHeading(curr_wp_local, prev_wp_local, curr_pos_local, lookahead_distance);
}
// 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_local, prev_wp_local, curr_pos_local,
lookahead_distance);
const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw);
// For logging
_rover_ackermann_guidance_status.lookahead_distance = lookahead_distance;
_rover_ackermann_guidance_status.heading_error = (heading_error * 180.f) / (M_PI_F);
_rover_ackermann_guidance_status.crosstrack_error = crosstrack_error.norm();
// Calculate desired steering
if (math::abs_t(heading_error) <= M_PI_2_F) {
return atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance);
@@ -330,47 +304,5 @@ float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_local,
return atanf(2 * wheel_base * (-1.0f + sinf(heading_error + M_PI_2_F)) /
lookahead_distance);
}
}
float RoverAckermannGuidance::calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
const Vector2f &curr_pos_local,
const float &lookahead_distance)
{
// Setup variables
const float line_segment_slope = (curr_wp_local(1) - prev_wp_local(1)) / (curr_wp_local(0) - prev_wp_local(0));
const float line_segment_rover_offset = prev_wp_local(1) - curr_pos_local(1) + line_segment_slope * (curr_pos_local(
0) - prev_wp_local(0));
const float a = -line_segment_slope;
const float c = -line_segment_rover_offset;
const float r = lookahead_distance;
const float x0 = -a * c / (a * a + 1.0f);
const float y0 = -c / (a * a + 1.0f);
// Calculate intersection points
if (c * c > r * r * (a * a + 1.0f) + FLT_EPSILON) { // No intersection points exist
return 0.f;
} else if (abs(c * c - r * r * (a * a + 1.0f)) < FLT_EPSILON) { // One intersection point exists
return atan2f(y0, x0);
} else { // Two intersetion points exist
const float d = r * r - c * c / (a * a + 1.0f);
const float mult = sqrt(d / (a * a + 1.0f));
const float ax = x0 + mult;
const float bx = x0 - mult;
const float ay = y0 - a * mult;
const float by = y0 + a * mult;
const Vector2f point1(ax, ay);
const Vector2f point2(bx, by);
const Vector2f distance1 = (curr_wp_local - curr_pos_local) - point1;
const Vector2f distance2 = (curr_wp_local - curr_pos_local) - point2;
// Return intersection point closer to current waypoint
if (distance1.norm_squared() < distance2.norm_squared()) {
return atan2f(ay, ax);
} else {
return atan2f(by, bx);
}
}
}
src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp
+2 -11
View File
@@ -35,6 +35,7 @@
// PX4 includes
#include <px4_platform_common/module_params.h>
#include <lib/pure_pursuit/PurePursuit.hpp>
// uORB includes
#include <uORB/Publication.hpp>
@@ -121,17 +122,6 @@ public:
const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max, const float &wheel_base,
const float &desired_speed, const float &vehicle_yaw);
/**
* @brief Return desired heading to the intersection point between a circle with a radius of
* lookahead_distance around the vehicle and a line segment from the previous to the current waypoint.
* @param curr_wp_local Current waypoint in local frame.
* @param prev_wp_local Previous waypoint in local frame.
* @param curr_pos_local Current position of the vehicle in local frame.
* @param lookahead_distance Radius of circle around vehicle.
*/
float calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local,
const float &lookahead_distance);
protected:
/**
* @brief Update the parameters of the module.
@@ -154,6 +144,7 @@ private:
MapProjection _global_local_proj_ref{}; // Transform global to local coordinates.
PurePursuit _pure_pursuit; // Pure pursuit library
// Rover variables
Vector2d _curr_pos{};