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

MC-auto: improve maximum vertical velocity computation

Browse Source
This commit is contained in:
bresch
2025-01-17 13:36:11 +01:00
parent a9214b3aa3
commit f996757009
3 changed files with 89 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/motion_planning/PositionSmoothing.cpp
+22 -39
View File
@@ -84,59 +84,42 @@ bool PositionSmoothing::_isTurning(const Vector3f &target) const
float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const
{
Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());
const Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());
math::trajectory::VehicleDynamicLimits config;
config.z_accept_rad = _vertical_acceptance_radius;
config.xy_accept_rad = _target_acceptance_radius;
config.max_acc_xy = _trajectory[0].getMaxAccel();
config.max_jerk = _trajectory[0].getMaxJerk();
config.max_speed_xy = _cruise_speed;
config.max_acc_xy_radius_scale = _horizontal_trajectory_gain;
math::trajectory::VehicleDynamicLimits config_xy;
config_xy.accept_rad = _target_acceptance_radius;
config_xy.max_acc = _trajectory[0].getMaxAccel();
config_xy.max_jerk = _trajectory[0].getMaxJerk();
config_xy.max_speed = _cruise_speed;
config_xy.max_acc_radius_scale = _horizontal_trajectory_gain;
// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
// (eg. Obstacle Avoidance)
Vector3f pos_to_waypoints[3] = {pos_traj, waypoints[1], waypoints[2]};
return math::trajectory::computeXYSpeedFromWaypoints<3>(pos_to_waypoints, config);
return math::trajectory::computeXYSpeedFromWaypoints<3>(pos_to_waypoints, config_xy);
}
float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
{
const Vector3f &start_position = {_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition()
};
const Vector3f &target = waypoints[1];
const Vector3f &next_target = waypoints[2];
const Vector3f pos_traj = {_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition()
};
const Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)};
const Vector2f target_xy_z = {target.xy().norm(), target(2)};
const Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)};
math::trajectory::VehicleDynamicLimits config_z;
config_z.accept_rad = _vertical_acceptance_radius;
config_z.max_acc = _trajectory[2].getMaxAccel();
config_z.max_jerk = _trajectory[2].getMaxJerk();
config_z.max_speed = _trajectory[2].getMaxVel();
config_z.max_acc_radius_scale = 1.f; // no scaling for Z
float arrival_z_speed = 0.0f;
const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f;
Vector3f pos_to_waypoints[3] = {pos_traj, waypoints[1], waypoints[2]};
if (target_next_different) {
const float alpha = acosf(Vector2f((target_xy_z - start_position_xy_z)).unit_or_zero().dot(
Vector2f((target_xy_z - next_target_xy_z)).unit_or_zero()));
const float safe_alpha = math::constrain(alpha, 0.f, M_PI_F - FLT_EPSILON);
float accel_tmp = _trajectory[2].getMaxAccel();
float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(safe_alpha, accel_tmp,
_vertical_acceptance_radius);
arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel());
}
const float distance_start_target = fabs(target(2) - start_position(2));
float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
_trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
distance_start_target, arrival_z_speed));
return max_speed;
return math::trajectory::computeZSpeedFromWaypoints<3>(pos_to_waypoints, config_z);
}
const Vector3f PositionSmoothing::_getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const
src/lib/motion_planning/TrajectoryConstraints.hpp
+40 -14
View File
@@ -46,16 +46,15 @@ using matrix::Vector3f;
using matrix::Vector2f;
struct VehicleDynamicLimits {
float z_accept_rad;
float xy_accept_rad;
float accept_rad;
float max_acc_xy;
float max_acc;
float max_jerk;
float max_speed_xy;
float max_speed;
// TODO: remove this
float max_acc_xy_radius_scale;
float max_acc_radius_scale;
};
/*
@@ -68,12 +67,12 @@ struct VehicleDynamicLimits {
*
*/
inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, const Vector3f &target,
const Vector3f &next_target, float exit_speed, const VehicleDynamicLimits &config)
const Vector3f &next_target, float exit_speed, const VehicleDynamicLimits &config_xy)
{
const float distance_target_next = (target - next_target).xy().norm();
const bool target_next_different = distance_target_next > 0.001f;
const bool waypoint_overlap = distance_target_next < config.xy_accept_rad;
const bool waypoint_overlap = distance_target_next < config_xy.accept_rad;
float speed_at_target = 0.0f;
@@ -83,15 +82,15 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co
const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot(
Vector2f((target - next_target).xy()).unit_or_zero()));
const float safe_alpha = constrain(alpha, 0.f, M_PI_F - FLT_EPSILON);
float accel_tmp = config.max_acc_xy_radius_scale * config.max_acc_xy;
float max_speed_in_turn = computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, config.xy_accept_rad);
speed_at_target = min(max_speed_in_turn, exit_speed, config.max_speed_xy);
float accel_tmp = config_xy.max_acc_radius_scale * config_xy.max_acc;
float max_speed_in_turn = computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, config_xy.accept_rad);
speed_at_target = min(max_speed_in_turn, exit_speed, config_xy.max_speed);
}
float start_to_target = (start_position - target).xy().norm();
float max_speed = computeMaxSpeedFromDistance(config.max_jerk, config.max_acc_xy, start_to_target, speed_at_target);
float max_speed = computeMaxSpeedFromDistance(config_xy.max_jerk, config_xy.max_acc, start_to_target, speed_at_target);
return min(config.max_speed_xy, max_speed);
return min(config_xy.max_speed, max_speed);
}
/*
@@ -105,7 +104,7 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co
* @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits
*/
template <int N>
float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config)
float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config_xy)
{
static_assert(N >= 2, "Need at least 2 points to compute speed");
@@ -116,7 +115,34 @@ float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDyna
max_speed = computeStartXYSpeedFromWaypoints(waypoints[i],
waypoints[i + 1],
waypoints[min(i + 2, N - 1)],
max_speed, config);
max_speed, config_xy);
}
return max_speed;
}
inline float computeStartZSpeedFromWaypoints(const Vector3f &start_position, const Vector3f &target,
float exit_speed, const VehicleDynamicLimits &config_z)
{
const float start_to_target = fabsf(target(2) - start_position(2));
const float accel_tmp = config_z.max_acc_radius_scale * config_z.max_acc;
const float max_speed = computeMaxSpeedFromDistance(config_z.max_jerk, accel_tmp, start_to_target, exit_speed);
return min(config_z.max_speed, max_speed);
}
template <int N>
float computeZSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config_z)
{
static_assert(N >= 2, "Need at least 2 points to compute speed");
float max_speed = 0.f;
// go backwards through the waypoints
for (int i = (N - 2); i >= 0; i--) {
max_speed = computeStartZSpeedFromWaypoints(waypoints[i],
waypoints[i + 1],
max_speed, config_z);
}
return max_speed;
src/lib/motion_planning/TrajectoryConstraintsTest.cpp
+27 -28
View File
@@ -8,7 +8,7 @@ using namespace math::trajectory;
class TrajectoryConstraintsTest : public ::testing::Test
{
public:
VehicleDynamicLimits config;
VehicleDynamicLimits config_xy;
Vector3f vehicle_location;
Vector3f target;
@@ -18,15 +18,14 @@ public:
void SetUp() override
{
config.z_accept_rad = 1.f;
config.xy_accept_rad = 0.99f;
config_xy.accept_rad = 0.99f;
config.max_acc_xy = 3.f;
config.max_jerk = 10.f;
config_xy.max_acc = 3.f;
config_xy.max_jerk = 10.f;
config.max_speed_xy = 10.f;
config_xy.max_speed = 10.f;
config.max_acc_xy_radius_scale = 0.8f;
config_xy.max_acc_radius_scale = 0.8f;
/*
* (20,20)
@@ -53,11 +52,11 @@ TEST_F(TrajectoryConstraintsTest, testStraight)
// WHEN: we get the speed for straight line travel
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy);
// THEN: it should be the same as speed directly to the end point
Vector3f direct_points[2] = {vehicle_location, next_target};
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config_xy);
EXPECT_FLOAT_EQ(through_speed, direct_speed);
}
@@ -72,11 +71,11 @@ TEST_F(TrajectoryConstraintsTest, testStraightNaN)
// WHEN: we get the speed for points which are NaN afterwards
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy);
// THEN: it should be the same as speed to the closer point
Vector3f direct_points[2] = {vehicle_location, target};
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config_xy);
EXPECT_FLOAT_EQ(through_speed, direct_speed);
}
@@ -86,15 +85,15 @@ TEST_F(TrajectoryConstraintsTest, testStraightLowJerkClose)
// GIVEN: 3 waypoints in straight line
next_target = target + 2.f * (target - vehicle_location);
target = vehicle_location + 0.05f * (next_target - vehicle_location);
config.max_jerk = 8.f;
config_xy.max_jerk = 8.f;
// WHEN: we get the speed for straight line travel
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy);
// THEN: it should be the same as speed directly to the end point
Vector3f direct_points[2] = {vehicle_location, next_target};
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config_xy);
EXPECT_FLOAT_EQ(through_speed, direct_speed);
}
@@ -107,11 +106,11 @@ TEST_F(TrajectoryConstraintsTest, testStraightMidClose)
// WHEN: we get the speed for straight line travel
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy);
// THEN: it should be the same as speed directly to the end point
Vector3f direct_points[2] = {vehicle_location, next_target};
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config_xy);
EXPECT_FLOAT_EQ(through_speed, direct_speed);
}
@@ -124,11 +123,11 @@ TEST_F(TrajectoryConstraintsTest, testStraightMidFar)
// WHEN: we get the speed for straight line travel
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy);
// THEN: it should be the same as speed directly to the end point
Vector3f direct_points[2] = {vehicle_location, next_target};
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config_xy);
EXPECT_FLOAT_EQ(through_speed, direct_speed);
}
@@ -141,11 +140,11 @@ TEST_F(TrajectoryConstraintsTest, test90Angle)
// WHEN: we get the speed for travel around the path
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy);
// THEN: it should be slightly faster than stopping at the intermediate point
Vector3f stop_points[2] = {vehicle_location, target};
float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config);
float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config_xy);
EXPECT_GT(through_speed, stop_speed); //faster
EXPECT_LT(through_speed, stop_speed * 1.03f); // but less than 3% faster
@@ -158,11 +157,11 @@ TEST_F(TrajectoryConstraintsTest, test45Angle)
// WHEN: we get the speed for travel around the path
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy);
// THEN: it should be slightly faster than stopping at the intermediate point
Vector3f stop_points[2] = {vehicle_location, target};
float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config);
float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config_xy);
EXPECT_GT(through_speed, stop_speed * 1.03f); // more than 3% faster
EXPECT_LT(through_speed, stop_speed * 1.06f); // but less than 6% faster
@@ -175,11 +174,11 @@ TEST_F(TrajectoryConstraintsTest, test10Angle)
// WHEN: we get the speed for travel around the path
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy);
// THEN: it should be slightly faster than stopping at the intermediate point
Vector3f stop_points[2] = {vehicle_location, target};
float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config);
float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config_xy);
EXPECT_GT(through_speed, stop_speed * 1.25f); // more than 25% faster
EXPECT_LT(through_speed, stop_speed * 1.3f); // but less than 30% faster
@@ -192,12 +191,12 @@ TEST_F(TrajectoryConstraintsTest, test10AngleFarNext)
// WHEN: we get the speed for travel around the path
Vector3f far_waypoints[3] = {vehicle_location, target, next_target};
float far_speed = computeXYSpeedFromWaypoints<3>(far_waypoints, config);
float far_speed = computeXYSpeedFromWaypoints<3>(far_waypoints, config_xy);
// THEN: it should be the same speed as a closer next waypoint at the same angle, since the bottleneck is the turn
next_target = Vector3f(30, 11.7, 5);
Vector3f close_waypoints[3] = {vehicle_location, target, next_target};
float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config);
float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config_xy);
EXPECT_FLOAT_EQ(far_speed, close_speed);
}
@@ -209,12 +208,12 @@ TEST_F(TrajectoryConstraintsTest, test10AngleCloseNext)
// WHEN: we get the speed for travel around the path
Vector3f close_waypoints[3] = {vehicle_location, target, next_target};
float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config);
float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config_xy);
// THEN: it should be slower than a further next waypoint at the same angle, since the bottleneck is the distance
next_target = Vector3f(30, 11.7, 5);
Vector3f normal_waypoints[3] = {vehicle_location, target, next_target};
float normal_speed = computeXYSpeedFromWaypoints<3>(normal_waypoints, config);
float normal_speed = computeXYSpeedFromWaypoints<3>(normal_waypoints, config_xy);
EXPECT_LT(close_speed, normal_speed);
}