Hi All,
I have been looking closely at the PositionSmoothing code in PX4 to get a better understanding of some of the odd behavior we are seeing in our drone. Specifically, strange behavior around loiter waypoints. Strange behavior aside, I realize I simply don’t understand what PositionSmoothing is trying to do? It looked at first like it was implementing an L1 controller, but then it deviates from that. Is there a guide or documentation anywhere on what the intentions of the algorithm are?
Some oddities I don’t understand:
- The calculated velocity’s magnitude and direction in the position smoothing don’t correspond to the same vector. The direction is pointing at the L1 point or the Target (depending on whether or not the craft is currently facing the target). The calculated velocity magnitude comes from assuming youre flying toward an arc within the radius of the current target waypoint. I don’t understand why this would make sense when you are using the L1 point as the crossing point instead of the target. In the traditional L1 algorithm, this vector toward the crossing point is used as an acceleration applied to a nominal velocity. that doesn’t seem to be what’s going on here. The relevant codeblock in PositionSmoothing.cpp
// Interface: A valid position setpoint generates a velocity target using conservative motion constraints.
// If a velocity is specified, that is used as a feedforward to track the position setpoint
// (ie. it assumes the position setpoint is moving at the specified velocity)
// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
auto &target = waypoints[1];
const bool xy_target_valid = PX4_ISFINITE(target(0)) && PX4_ISFINITE(target(1));
const bool z_target_valid = PX4_ISFINITE(target(2));
Vector3f velocity_setpoint = feedforward_velocity_setpoint;
if (xy_target_valid && z_target_valid) {
// Use 3D position setpoint to generate a 3D velocity setpoint
Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());
const Vector3f crossing_point = is_single_waypoint ? target : _getCrossingPoint(position, waypoints);
const Vector3f u_pos_traj_to_dest{(crossing_point - pos_traj).unit_or_zero()};
float xy_speed = _getMaxXYSpeed(waypoints);
const float z_speed = _getMaxZSpeed(waypoints);
if (!is_single_waypoint && _isTurning(target)) {
// Limit speed during a turn
xy_speed = math::min(_max_speed_previous, xy_speed);
} else {
_max_speed_previous = xy_speed;
}
Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed);
math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f);
math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f);
for (int i = 0; i < 3; i++) {
// If available, use the existing velocity as a feedforward, otherwise replace it
if (PX4_ISFINITE(velocity_setpoint(i))) {
velocity_setpoint(i) += vel_sp_constrained(i);
} else {
velocity_setpoint(i) = vel_sp_constrained(i);
}
}
}
- The velocity calculations to compute the speeds always assume that the next waypoint is the final waypoint and that it should be stopped at. If you work through the algorithm below, this is why the craft seems to not fly smoothly through waypoints? I am unsure. Either way, I don’t understand how this was supposed to work. Relevant codeblock in TrajectoryConstraints.hpp
/*
* Compute the maximum allowed speed at the waypoint assuming that we want to
* connect the two lines (current-target and target-next)
* with a tangent circle with constant speed and desired centripetal acceleration: a_centripetal = speed^2 / radius
* The circle should in theory start and end at the intersection of the lines and the waypoint's acceptance radius.
* This is not exactly true in reality since Navigator switches the waypoint so we have to take in account that
* the real acceptance radius is smaller.
*
*/
inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, const Vector3f &target,
const Vector3f &next_target, float exit_speed, const VehicleDynamicLimits &config)
{
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 has_reached_altitude = fabsf(target(2) - start_position(2)) < config.z_accept_rad;
const bool altitude_stays_same = fabsf(next_target(2) - target(2)) < config.z_accept_rad;
float speed_at_target = 0.0f;
if (target_next_different &&
!waypoint_overlap &&
has_reached_altitude &&
altitude_stays_same
) {
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 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);
return min(config.max_speed_xy, max_speed);
}
/*
* This function computes the maximum speed XY that can be travelled, given a set of waypoints and vehicle dynamics
*
* The first waypoint should be the starting location, and the later waypoints the desired points to be followed.
*
* @param waypoints the list of waypoints to be followed, the first of which should be the starting location
* @param config the vehicle dynamic limits
*
* @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits
*/
template <size_t N>
float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config)
{
static_assert(N >= 2, "Need at least 2 points to compute speed");
float max_speed = 0.f;
for (size_t j = 0; j < N - 1; j++) {
size_t i = N - 2 - j;
max_speed = computeStartXYSpeedFromWaypoints(waypoints[i],
waypoints[i + 1],
waypoints[min(i + 2, N - 1)],
max_speed, config);
}
return max_speed;
}
I cannot tell if I am misunderstanding the intentions behind the algorithms and would appreciate guidance!