mirror of https://github.com/ArduPilot/ardupilot
AP_Math: Control: Add directional based acceleration limit
AP_Math: Control: Add directional based acceleration limit
This commit is contained in:
Leonard Hall
Randy Mackay
parent
b34a7e46a8
commit
1e124ca957
|
|
@ -29,9 +29,9 @@
|
|||
#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.
|
||||
// the velocity is not moved in the direction of limit if limit is not set to zero
|
||||
// limit - specifies if the system is unable to continue to accelerate
|
||||
// vel_error - specifies the direction of the velocity error useded in limit handling
|
||||
// the velocity is not moved in the direction of limit if limit is not set to zero.
|
||||
// limit - specifies if the system is unable to continue to accelerate.
|
||||
// vel_error - specifies the direction of the velocity error useded in limit handling.
|
||||
void update_vel_accel(float& vel, float accel, float dt, float limit, float vel_error)
|
||||
{
|
||||
const float delta_vel = accel * dt;
|
||||
|
|
@ -42,9 +42,9 @@ void update_vel_accel(float& vel, float accel, float dt, float limit, float vel_
|
|||
}
|
||||
|
||||
// update_pos_vel_accel - single axis projection of position and velocity forward in time based on a time step of dt and acceleration of accel.
|
||||
// the position and velocity is not moved in the direction of limit if limit is not set to zero
|
||||
// limit - specifies if the system is unable to continue to accelerate
|
||||
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling
|
||||
// the position and velocity is not moved in the direction of limit if limit is not set to zero.
|
||||
// limit - specifies if the system is unable to continue to accelerate.
|
||||
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling.
|
||||
void update_pos_vel_accel(postype_t& pos, float& vel, float accel, float dt, float limit, float pos_error, float vel_error)
|
||||
{
|
||||
// move position and velocity forward by dt if it does not increase error when limited.
|
||||
|
|
@ -58,9 +58,9 @@ void update_pos_vel_accel(postype_t& pos, float& vel, float accel, float dt, flo
|
|||
}
|
||||
|
||||
// update_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
|
||||
// the velocity is not moved in the direction of limit if limit is not set to zero
|
||||
// limit - specifies if the system is unable to continue to accelerate
|
||||
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling
|
||||
// the velocity is not moved in the direction of limit if limit is not set to zero.
|
||||
// limit - specifies if the system is unable to continue to accelerate.
|
||||
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling.
|
||||
void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& vel_error)
|
||||
{
|
||||
// increase velocity by acceleration * dt if it does not increase error when limited.
|
||||
|
|
@ -77,9 +77,9 @@ void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, const V
|
|||
}
|
||||
|
||||
// update_pos_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
|
||||
// the position and velocity is not moved in the direction of limit if limit is not set to zero
|
||||
// limit - specifies if the system is unable to continue to accelerate
|
||||
// pos_error and vel_error - specifies the direction of the velocity error used in limit handling
|
||||
// the position and velocity is not moved in the direction of limit if limit is not set to zero.
|
||||
// limit - specifies if the system is unable to continue to accelerate.
|
||||
// pos_error and vel_error - specifies the direction of the velocity error used in limit handling.
|
||||
void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& pos_error, const Vector2f& vel_error)
|
||||
{
|
||||
// move position and velocity forward by dt.
|
||||
|
|
@ -100,12 +100,12 @@ void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel
|
|||
/* shape_accel calculates a jerk limited path from the current acceleration to an input acceleration.
|
||||
The function takes the current acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
|
||||
The kinematic path is constrained by :
|
||||
acceleration limits - accel_min, accel_max,
|
||||
time constant - tc.
|
||||
acceleration limits - accel_min, 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 variable accel to follow a jerk limited kinematic path to accel_input
|
||||
The function alters the variable accel to follow a jerk limited kinematic path to accel_input.
|
||||
*/
|
||||
void shape_accel(float accel_input, float& accel,
|
||||
float jerk_max, float dt)
|
||||
|
|
@ -141,17 +141,16 @@ void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
|
|||
accel.y = accel_2f.y;
|
||||
}
|
||||
|
||||
|
||||
/* shape_vel_accel 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.
|
||||
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 variable accel to follow a jerk limited kinematic path to vel_input and accel_input
|
||||
The function alters the variable accel to follow a jerk limited kinematic path to vel_input and accel_input.
|
||||
The accel_max limit can be removed by setting it to zero.
|
||||
*/
|
||||
void shape_vel_accel(float vel_input, float accel_input,
|
||||
|
|
@ -246,13 +245,13 @@ void shape_vel_accel_xy(const Vector2f &vel_input, const Vector2f& accel_input,
|
|||
/* shape_pos_vel_accel 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.
|
||||
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 variable accel to follow a jerk limited kinematic path to pos_input, vel_input and accel_input
|
||||
The function alters the variable accel to follow a jerk limited kinematic path to pos_input, vel_input and accel_input.
|
||||
The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
|
||||
*/
|
||||
void shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input,
|
||||
|
|
@ -324,7 +323,45 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input
|
|||
shape_vel_accel_xy(vel_target, accel_input, vel, accel, accel_max, jerk_max, dt, limit_total_accel);
|
||||
}
|
||||
|
||||
// proportional controller with piecewise sqrt sections to constrain second derivative
|
||||
/* limit_accel_xy limits the acceleration to prioritise acceleration perpendicular to the provided velocity vector.
|
||||
Input parameters are:
|
||||
vel is the velocity vector used to define the direction acceleration limit is biased in.
|
||||
accel is the acceleration vector to be limited.
|
||||
accel_max is the maximum length of the acceleration vector after being limited.
|
||||
Returns true when accel vector has been limited.
|
||||
*/
|
||||
bool limit_accel_xy(const Vector2f& vel, Vector2f& accel, float accel_max)
|
||||
{
|
||||
// check accel_max is defined
|
||||
if (!is_positive(accel_max)) {
|
||||
return false;
|
||||
}
|
||||
// limit acceleration to accel_max while prioritizing cross track acceleration
|
||||
if (accel.length_squared() > sq(accel_max)) {
|
||||
if (vel.is_zero()) {
|
||||
// We do not have a direction of travel so do a simple vector length limit
|
||||
accel.limit_length(accel_max);
|
||||
} else {
|
||||
// calculate acceleration in the direction of and perpendicular to the velocity input
|
||||
const Vector2f vel_input_unit = vel.normalized();
|
||||
// acceleration in the direction of travel
|
||||
float accel_dir = vel_input_unit * accel;
|
||||
// cross track acceleration
|
||||
Vector2f accel_cross = accel - (vel_input_unit * accel_dir);
|
||||
if (accel_cross.limit_length(accel_max)) {
|
||||
accel_dir = 0.0;
|
||||
} else {
|
||||
float accel_max_dir = safe_sqrt(sq(accel_max) - accel_cross.length_squared());
|
||||
accel_dir = constrain_float(accel_dir, -accel_max_dir, accel_max_dir);
|
||||
}
|
||||
accel = accel_cross + vel_input_unit * accel_dir;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// sqrt_controller calculates the correction based on a 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;
|
||||
|
|
@ -359,7 +396,7 @@ float sqrt_controller(float error, float p, float second_ord_lim, float dt)
|
|||
}
|
||||
}
|
||||
|
||||
// proportional controller with piecewise sqrt sections to constrain second derivative
|
||||
// sqrt_controller calculates the correction based on a 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();
|
||||
|
|
@ -371,7 +408,8 @@ Vector2f sqrt_controller(const Vector2f& error, float p, float second_ord_lim, f
|
|||
return error * (correction_length / error_length);
|
||||
}
|
||||
|
||||
// inverse of the sqrt controller. calculates the input (aka error) to the sqrt_controller required to achieve a given output
|
||||
// inv_sqrt_controller calculates the inverse of the sqrt controller.
|
||||
// This function 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)) {
|
||||
|
|
@ -384,7 +422,7 @@ float inv_sqrt_controller(float output, float p, float D_max)
|
|||
return 0.0;
|
||||
}
|
||||
|
||||
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
|
||||
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function.
|
||||
const float linear_velocity = D_max / p;
|
||||
|
||||
if (fabsf(output) < linear_velocity) {
|
||||
|
|
@ -397,13 +435,13 @@ float inv_sqrt_controller(float output, float p, float D_max)
|
|||
return is_positive(output) ? stopping_dist : -stopping_dist;
|
||||
}
|
||||
|
||||
// calculate the stopping distance for the square root controller based deceleration path
|
||||
// stopping_distance calculates the stopping distance for the square root controller based deceleration path.
|
||||
float stopping_distance(float velocity, float p, float accel_max)
|
||||
{
|
||||
return inv_sqrt_controller(velocity, p, accel_max);
|
||||
}
|
||||
|
||||
// calculate the maximum acceleration or velocity in a given direction
|
||||
// kinematic_limit calculates 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)
|
||||
{
|
||||
|
|
@ -440,9 +478,9 @@ float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float m
|
|||
return fabsf(max_z_neg/direction.z);
|
||||
}
|
||||
|
||||
// calculate the expo function on the normalised input
|
||||
// input must be in the range of -1 to 1
|
||||
// expo should be less than 1.0 but limited to be less than 0.95
|
||||
// input_expo calculates the expo function on the normalised input.
|
||||
// The input must be in the range of -1 to 1.
|
||||
// The expo should be less than 1.0 but limited to be less than 0.95.
|
||||
float input_expo(float input, float expo)
|
||||
{
|
||||
input = constrain_float(input, -1.0, 1.0);
|
||||
|
|
|
|||
|
|
@ -26,78 +26,82 @@ typedef Vector3f Vector3p;
|
|||
*/
|
||||
|
||||
// update_vel_accel - single axis projection of velocity, vel, forwards in time based on a time step of dt and acceleration of accel.
|
||||
// the velocity is not moved in the direction of limit if limit is not set to zero
|
||||
// limit - specifies if the system is unable to continue to accelerate
|
||||
// vel_error - specifies the direction of the velocity error useded in limit handling
|
||||
// the velocity is not moved in the direction of limit if limit is not set to zero.
|
||||
// limit - specifies if the system is unable to continue to accelerate.
|
||||
// vel_error - specifies the direction of the velocity error useded in limit handling.
|
||||
void update_vel_accel(float& vel, float accel, float dt, float limit, float vel_error);
|
||||
|
||||
// update_pos_vel_accel - single axis projection of position and velocity forward in time based on a time step of dt and acceleration of accel.
|
||||
// the position and velocity is not moved in the direction of limit if limit is not set to zero
|
||||
// limit - specifies if the system is unable to continue to accelerate
|
||||
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling
|
||||
// the position and velocity is not moved in the direction of limit if limit is not set to zero.
|
||||
// limit - specifies if the system is unable to continue to accelerate.
|
||||
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling.
|
||||
void update_pos_vel_accel(postype_t & pos, float& vel, float accel, float dt, float limit, float pos_error, float vel_error);
|
||||
|
||||
// update_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
|
||||
// the velocity is not moved in the direction of limit if limit is not set to zero
|
||||
// limit - specifies if the system is unable to continue to accelerate
|
||||
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling
|
||||
// the velocity is not moved in the direction of limit if limit is not set to zero.
|
||||
// limit - specifies if the system is unable to continue to accelerate.
|
||||
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling.
|
||||
void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& vel_error);
|
||||
|
||||
// update_pos_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
|
||||
// the position and velocity is not moved in the direction of limit if limit is not set to zero
|
||||
// limit - specifies if the system is unable to continue to accelerate
|
||||
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling
|
||||
// the position and velocity is not moved in the direction of limit if limit is not set to zero.
|
||||
// limit - specifies if the system is unable to continue to accelerate.
|
||||
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling.
|
||||
void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& pos_error, const Vector2f& vel_error);
|
||||
|
||||
/* shape_accel calculates a jerk limited path from the current acceleration to an input acceleration.
|
||||
The function takes the current acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
|
||||
The kinematic path is constrained by :
|
||||
acceleration limits - accel_min, accel_max,
|
||||
time constant - tc.
|
||||
acceleration limits - accel_min, 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 variable accel to follow a jerk limited kinematic path to accel_input
|
||||
The function alters the variable accel to follow a jerk limited kinematic path to accel_input.
|
||||
*/
|
||||
void shape_accel(float accel_input, float& accel,
|
||||
float jerk_max, float dt);
|
||||
|
||||
// 2D version
|
||||
void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
|
||||
float jerk_max, float dt);
|
||||
|
||||
void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
|
||||
float jerk_max, float dt);
|
||||
|
||||
/* shape_vel calculates a jerk limited path from the current velocity and acceleration to an input velocity.
|
||||
The function takes the current velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
|
||||
/* shape_vel_accel 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 :
|
||||
velocity limits - vel_min, vel_max,
|
||||
acceleration limits - accel_min, accel_max,
|
||||
time constant - tc.
|
||||
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 variable accel to follow a jerk limited kinematic path to vel_input and accel_input
|
||||
The function alters the variable accel to follow a jerk limited kinematic path to vel_input and accel_input.
|
||||
The accel_max limit can be removed by setting it to zero.
|
||||
*/
|
||||
void shape_vel_accel(float vel_input, float accel_input,
|
||||
float vel, float& accel,
|
||||
float accel_min, float accel_max,
|
||||
float jerk_max, float dt, bool limit_total_accel);
|
||||
|
||||
// 2D version
|
||||
void shape_vel_accel_xy(const Vector2f &vel_input1, const Vector2f& accel_input,
|
||||
const Vector2f& vel, Vector2f& accel,
|
||||
float accel_max, float jerk_max, float dt, bool limit_total_accel);
|
||||
|
||||
/* shape_pos_vel calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
|
||||
/* shape_pos_vel_accel 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.
|
||||
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 variable accel to follow a jerk limited kinematic path to pos_input, vel_input and accel_input
|
||||
The function alters the variable accel to follow a jerk limited kinematic path to pos_input, vel_input and accel_input.
|
||||
The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
|
||||
*/
|
||||
void shape_pos_vel_accel(const postype_t pos_input, float vel_input, float accel_input,
|
||||
const postype_t pos, float vel, float& accel,
|
||||
|
|
@ -105,28 +109,39 @@ void shape_pos_vel_accel(const postype_t pos_input, float vel_input, float accel
|
|||
float accel_min, float accel_max,
|
||||
float jerk_max, float dt, bool limit_total_accel);
|
||||
|
||||
// 2D version
|
||||
void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input, const Vector2f& accel_input,
|
||||
const Vector2p& pos, const Vector2f& vel, Vector2f& accel,
|
||||
float vel_max, float accel_max,
|
||||
float jerk_max, float dt, bool limit_total_accel);
|
||||
|
||||
// proportional controller with piecewise sqrt sections to constrain second derivative
|
||||
/* limit_accel_xy limits the acceleration to prioritise acceleration perpendicular to the provided velocity vector.
|
||||
Input parameters are:
|
||||
vel is the velocity vector used to define the direction acceleration limit is biased in.
|
||||
accel is the acceleration vector to be limited.
|
||||
accel_max is the maximum length of the acceleration vector after being limited.
|
||||
Returns true when accel vector has been limited.
|
||||
*/
|
||||
bool limit_accel_xy(const Vector2f& vel, Vector2f& accel, float accel_max);
|
||||
|
||||
// sqrt_controller calculates the correction based on a 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
|
||||
// sqrt_controller calculates the correction based on a 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
|
||||
// inv_sqrt_controller calculates the inverse of the sqrt controller.
|
||||
// This function 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
|
||||
// stopping_distance calculates the stopping distance for the square root controller based deceleration path.
|
||||
float stopping_distance(float velocity, float p, float accel_max);
|
||||
|
||||
// calculate the maximum acceleration or velocity in a given direction
|
||||
// kinematic_limit calculates 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);
|
||||
|
||||
// calculate the expo function on the normalised input
|
||||
// input must be in the range of -1 to 1
|
||||
// expo should be less than 1.0 but limited to be less than 0.95
|
||||
// input_expo calculates the expo function on the normalised input.
|
||||
// The input must be in the range of -1 to 1.
|
||||
// The expo should be less than 1.0 but limited to be less than 0.95.
|
||||
float input_expo(float input, float expo);
|
||||
|
|
|
|||
Loading…
Reference in New Issue