Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: Control: improve real time S-Curve response to small inputs

Browse Source
This commit is contained in:
Leonard Hall 2022-01-31 10:58:05 +10:30 committed by Randy Mackay
parent a7b43b0ded
commit a47fc65511
1 changed files with 29 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_Math/control.cpp
View File

@ -25,7 +25,6 @@
#include <AP_InternalError/AP_InternalError.h>
// control default definitions
#define CONTROL_TIME_CONSTANT_RATIO 4.0 // time constant to ensure stable kinematic path generation
#define CORNER_ACCELERATION_RATIO 1.0/safe_sqrt(2.0) // acceleration reduction to enable zero overshoot corners
// update_vel_accel - single axis projection of velocity, vel, forwards in time based on a time step of dt and acceleration of accel.
@ -164,14 +163,22 @@ void shape_vel_accel(float vel_input, float accel_input,
return;
}
// Calculate time constants and limits to ensure stable operation
const float KPa = jerk_max / accel_max;
// velocity error to be corrected
float vel_error = vel_input - vel;
// Calculate time constants and limits to ensure stable operation
float KPa;
// The direction of acceleration limit is the same as the velocity error.
// This is because the velocity error is negative when slowing down while
// closing a positive position error.
if (is_positive(vel_error)) {
KPa = jerk_max / accel_max;
} else {
KPa = jerk_max / (-accel_min);
}
// acceleration to correct velocity
float accel_target = vel_error * KPa;
float accel_target = sqrt_controller(vel_error, KPa, jerk_max, dt);
// constrain correction acceleration from accel_min to accel_max
accel_target = constrain_float(accel_target, accel_min, accel_max);
@ -205,7 +212,7 @@ void shape_vel_accel_xy(const Vector2f& vel_input, const Vector2f& accel_input,
const Vector2f vel_error = vel_input - vel;
// acceleration to correct velocity
Vector2f accel_target = vel_error * KPa;
Vector2f accel_target = sqrt_controller(vel_error, KPa, jerk_max, dt);
// limit correction acceleration to accel_max
if (vel_input.is_zero()) {
@ -266,13 +273,24 @@ void shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input
return;
}
// Calculate time constants and limits to ensure stable operation
const float KPv = jerk_max / (CONTROL_TIME_CONSTANT_RATIO * MAX(-accel_min, accel_max));
const float accel_tc_max = MIN(-accel_min, accel_max) * (1.0 - 1.0 / CONTROL_TIME_CONSTANT_RATIO);
// position error to be corrected
float pos_error = pos_input - pos;
float accel_tc_max;
float KPv;
// Calculate time constants and limits to ensure stable operation
// The negative acceleration limit is used here because the square root controller
// manages the approach to the setpoint. Therefore the acceleration is in the opposite
// direction to the position error.
if (is_positive(pos_error)) {
accel_tc_max = -0.5 * accel_min;
KPv = 0.5 * jerk_max / (-accel_min);
} else {
accel_tc_max = 0.5 * accel_max;
KPv = 0.5 * jerk_max / accel_max;
}
// velocity to correct position
float vel_target = sqrt_controller(pos_error, KPv, accel_tc_max, dt);
@ -300,9 +318,9 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input
}
// Calculate time constants and limits to ensure stable operation
const float KPv = jerk_max / (CONTROL_TIME_CONSTANT_RATIO * accel_max);
const float KPv = 0.5 * jerk_max / accel_max;
// reduce breaking acceleration to support cornering without overshooting the stopping point
const float accel_tc_max = CORNER_ACCELERATION_RATIO * accel_max * (1.0 - 1.0 / CONTROL_TIME_CONSTANT_RATIO);
const float accel_tc_max = 0.5 * accel_max;
// position error to be corrected
Vector2f pos_error = (pos_input - pos).tofloat();