From 3bb048279584b093c1f1ec01b65bdb92b4b787af Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 20 Jan 2021 10:39:39 +0900 Subject: [PATCH] AP_Math: add update_pos_vel_accel to control also add shape_vel, shape_pos_vel and stopping_distance also add calculation of kinematic limits --- libraries/AP_Math/control.cpp | 227 +++++++++++++++++++++++++++++++++- libraries/AP_Math/control.h | 45 ++++++- 2 files changed, 269 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp index fc504d0e45..fb4204b657 100644 --- a/libraries/AP_Math/control.cpp +++ b/libraries/AP_Math/control.cpp @@ -23,7 +23,114 @@ #include "vector2.h" #include "vector3.h" -// Proportional controller with piecewise sqrt sections to constrain second derivative +// control default definitions +#define CONTROL_TIME_CONSTANT_RATIO 4.0f // minimum horizontal acceleration in cm/s/s - used for sanity checking acceleration in leash length calculation + +// update_pos_vel_accel - single axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel. +void update_pos_vel_accel(float& pos, float& vel, float accel, float dt) +{ + // move position and velocity forward by dt. + pos = pos + vel * dt + accel * 0.5f * sq(dt); + vel = vel + accel * dt; +} + +/* shape_vel and shape_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity. + The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. + The kinematic path is constrained by : + maximum velocity - vel_max, + maximum acceleration - accel_max, + time constant - tc. + The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. + The time constant also defines the time taken to achieve the maximum acceleration. + The time constant must be positive. + The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time. + The accel_max limit can be removed by setting it to zero. +*/ +void shape_vel(float& vel_input, float vel, float& accel, float accel_max, float tc, float dt) +{ + // sanity check tc + if (!is_positive(tc)) { + return; + } + + // Calculate time constants and limits to ensure stable operation + const float KPa = 1.0 / tc; + const float jerk_max = accel_max * KPa; + + // velocity error to be corrected + float vel_error = vel_input - vel; + + // acceleration to correct velocity + const float accel_target = vel_error * KPa; + + // jerk limit acceleration change + float accel_delta = accel_target - accel; + if (is_positive(jerk_max)) { + accel_delta = constrain_float(accel_delta, -jerk_max * dt, jerk_max * dt); + } + accel += accel_delta; + + // limit acceleration to accel_max + if (is_positive(accel_max)) { + accel = constrain_float(accel, -accel_max, accel_max); + } + + // Calculate maximum pos_input and vel_input based on limited system + vel_error = accel / KPa; + vel_input = vel_error + vel; +} + + +/* shape_pos_vel calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity. + The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. + The kinematic path is constrained by : + maximum velocity - vel_max, + maximum acceleration - accel_max, + time constant - tc. + The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. + The time constant also defines the time taken to achieve the maximum acceleration. + The time constant must be positive. + The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time. + The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero. +*/ +void shape_pos_vel(float& pos_input, float vel_input, float pos, float vel, float& accel, float vel_max, float vel_correction_max, float accel_max, float tc, float dt) +{ + // sanity check tc + if (!is_positive(tc)) { + return; + } + + // Calculate time constants and limits to ensure stable operation + const float KPv = 1.0 / (CONTROL_TIME_CONSTANT_RATIO*tc); + const float accel_tc_max = accel_max*(1-1.0f/CONTROL_TIME_CONSTANT_RATIO); + + // position error to be corrected + float pos_error = pos_input - pos; + + // velocity to correct position + float vel_target = sqrt_controller(pos_error, KPv, accel_tc_max, dt); + + // limit velocity correction to vel_correction_max + if (is_positive(vel_correction_max)) { + vel_target = constrain_float(vel_target, -vel_correction_max, vel_correction_max); + } + + // velocity correction with input velocity + vel_target += vel_input; + + // limit velocity to vel_max + if (is_positive(vel_max)) { + vel_target = constrain_float(vel_target, -vel_max, vel_max); + } + + shape_vel(vel_target, vel, accel, accel_max, tc, dt); + + vel_target -= vel_input; + pos_error = stopping_distance(vel_target, KPv, accel_tc_max); + pos_input = pos_error + pos; +} + +// proportional controller with piecewise sqrt sections to constrain second derivative float sqrt_controller(float error, float p, float second_ord_lim, float dt) { float correction_rate; @@ -41,7 +148,7 @@ float sqrt_controller(float error, float p, float second_ord_lim, float dt) } } else { // Both the P and second order limit have been defined. - float linear_dist = second_ord_lim / sq(p); + const float linear_dist = second_ord_lim / sq(p); if (error > linear_dist) { correction_rate = safe_sqrt(2.0f * second_ord_lim * (error - (linear_dist / 2.0f))); } else if (error < -linear_dist) { @@ -58,6 +165,80 @@ float sqrt_controller(float error, float p, float second_ord_lim, float dt) } } +// proportional controller with piecewise sqrt sections to constrain second derivative +Vector2f sqrt_controller(const Vector2f& error, float p, float second_ord_lim, float dt) +{ + const float error_length = error.length(); + const float correction_length = sqrt_controller(error_length, p, second_ord_lim, dt); + if (is_positive(error_length)) { + return error * (correction_length / error_length); + } else { + return Vector2f(0.0f, 0.0f); + } +} + +// inverse of the sqrt controller. calculates the input (aka error) to the sqrt_controller required to achieve a given output +float inv_sqrt_controller(float output, float p, float D_max) +{ + if (!is_positive(D_max) && is_zero(p)) { + return 0.0f; + } + + if (!is_positive(D_max)) { + // second order limit is zero or negative. + return output / p; + } + + if (is_zero(p)) { + // P term is zero but we have a second order limit. + if (is_positive(D_max)) { + return sq(output)/(2*D_max); + } else { + return -sq(output)/(2*D_max); + } + } + + // both the P and second order limit have been defined. + float linear_out = D_max / p; + if (output > linear_out) { + if (is_positive(D_max)) { + return sq(output)/(2*D_max) + D_max/(4*sq(p)); + } else { + return -sq(output)/(2*D_max) + D_max/(4*sq(p)); + } + } + + return output / p; +} + +// calculate the stopping distance for the square root controller based deceleration path +float stopping_distance(float velocity, float p, float accel_max) +{ + if (is_positive(accel_max) && is_zero(p)) { + return (velocity * velocity) / (2.0f * accel_max); + } else if ((is_negative(accel_max) || is_zero(accel_max)) && !is_zero(p)) { + return velocity / p; + } else if ((is_negative(accel_max) || is_zero(accel_max)) && is_zero(p)) { + return 0.0f; + } + + // calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function + const float linear_velocity = accel_max / p; + + if (fabsf(velocity) < linear_velocity) { + // if our current velocity is below the cross-over point we use a linear function + return velocity / p; + } else { + const float linear_dist = accel_max / sq(p); + const float stopping_dist = (linear_dist * 0.5f) + sq(velocity) / (2.0f * accel_max); + if (is_positive(velocity)) { + return stopping_dist; + } else { + return -stopping_dist; + } + } +} + // limit vector to a given length, returns true if vector was limited bool limit_vector_length(float &vector_x, float &vector_y, float max_length) { @@ -69,3 +250,45 @@ bool limit_vector_length(float &vector_x, float &vector_y, float max_length) } return false; } + +// calculate the maximum acceleration or velocity in a given direction +// based on horizontal and vertical limits. +float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float max_z_neg) +{ + max_xy = fabsf(max_xy); + max_z_pos = fabsf(max_z_pos); + max_z_neg = fabsf(max_z_neg); + + if (is_zero(direction.length_squared())) { + return 0.0f; + } + + direction.normalize(); + const float xy_length = Vector2f{direction.x, direction.y}.length(); + + if (is_zero(xy_length)) { + if (is_positive(direction.z)) { + return max_z_pos; + } else { + return max_z_neg; + } + } else if (is_zero(direction.z)) { + return max_xy; + } else { + const float slope = direction.z/xy_length; + if (is_positive(slope)) { + if (fabsf(slope) < max_z_pos/max_xy) { + return max_xy/xy_length; + } else { + return fabsf(max_z_pos/direction.z); + } + } else { + if (fabsf(slope) < max_z_neg/max_xy) { + return max_xy/xy_length; + } else { + return fabsf(max_z_neg/direction.z); + } + } + } + return 0.0f; +} diff --git a/libraries/AP_Math/control.h b/libraries/AP_Math/control.h index e026eddef8..8311939c4e 100644 --- a/libraries/AP_Math/control.h +++ b/libraries/AP_Math/control.h @@ -4,8 +4,51 @@ common controller helper functions */ -// Proportional controller with piecewise sqrt sections to constrain second derivative +// update_pos_vel_accel projects the position and velocity, pos and vel, forward in time based on a time step of dt and acceleration of accel. +// update_pos_vel_accel - single axis projection. +void update_pos_vel_accel(float& pos, float& vel, float accel, float dt); + +/* shape_vel calculates a jerk limited path from the current position, velocity and acceleration to an input velocity. + The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. + The kinematic path is constrained by : + maximum velocity - vel_max, + maximum acceleration - accel_max, + time constant - tc. + The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. + The time constant also defines the time taken to achieve the maximum acceleration. + The time constant must be positive. + The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time. +*/ +void shape_vel(float& vel_input, float vel, float& accel, float accel_max, float tc, float dt); + +/* shape_pos_vel calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity. + The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. + The kinematic path is constrained by : + maximum velocity - vel_max, + maximum acceleration - accel_max, + time constant - tc. + The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. + The time constant also defines the time taken to achieve the maximum acceleration. + The time constant must be positive. + The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time. +*/ +void shape_pos_vel(float& pos_input, float vel_input, float pos, float vel, float& accel, float vel_max, float vel_correction_max, float accel_max, float tc, float dt); + +// proportional controller with piecewise sqrt sections to constrain second derivative float sqrt_controller(float error, float p, float second_ord_lim, float dt); +// proportional controller with piecewise sqrt sections to constrain second derivative +Vector2f sqrt_controller(const Vector2f& error, float p, float second_ord_lim, float dt); + +// inverse of the sqrt controller. calculates the input (aka error) to the sqrt_controller required to achieve a given output +float inv_sqrt_controller(float output, float p, float D_max); + +// calculate the stopping distance for the square root controller based deceleration path +float stopping_distance(float velocity, float p, float accel_max); + // limit vector to a given length, returns true if vector was limited bool limit_vector_length(float &vector_x, float &vector_y, float max_length); + +// calculate the maximum acceleration or velocity in a given direction +// based on horizontal and vertical limits. +float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float max_z_neg);