aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttitudeController: Thrust Vector attitude control

Browse Source
This commit is contained in:
Leonard Hall 2021-04-15 21:54:43 +09:30 committed by Randy Mackay
parent 6af0f460af
commit 1fb2a95486
2 changed files with 155 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

157
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -484,7 +484,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
} else {
// When feedforward is not enabled, the quaternion is calculated and is input into the target and the feedforward rate is zeroed.
Quaternion attitude_target_update;
attitude_target_update.from_axis_angle(Vector3f(roll_rate_rads * _dt, pitch_rate_rads * _dt, yaw_rate_rads * _dt));
attitude_target_update.from_axis_angle(Vector3f{roll_rate_rads * _dt, pitch_rate_rads * _dt, yaw_rate_rads * _dt});
_attitude_target = _attitude_target * attitude_target_update;
_attitude_target.normalize();
@ -541,7 +541,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
}
Vector3f gyro_latest = _ahrs.get_gyro_latest();
attitude_ang_error_update_quat.from_axis_angle(Vector3f((_ang_vel_target.x-gyro_latest.x) * _dt, (_ang_vel_target.y-gyro_latest.y) * _dt, (_ang_vel_target.z-gyro_latest.z) * _dt));
attitude_ang_error_update_quat.from_axis_angle(Vector3f{(_ang_vel_target.x-gyro_latest.x) * _dt, (_ang_vel_target.y-gyro_latest.y) * _dt, (_ang_vel_target.z-gyro_latest.z) * _dt});
_attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error;
// Compute acceleration-limited body frame rates
@ -584,7 +584,7 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste
// rotate attitude target by desired step
Quaternion attitude_target_update;
attitude_target_update.from_axis_angle(Vector3f(roll_step_rads, pitch_step_rads, yaw_step_rads));
attitude_target_update.from_axis_angle(Vector3f{roll_step_rads, pitch_step_rads, yaw_step_rads});
_attitude_target = _attitude_target * attitude_target_update;
_attitude_target.normalize();
@ -599,6 +599,130 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste
attitude_controller_run_quat();
}
// Command a Quaternion attitude with feedforward and smoothing
void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds)
{
// Convert from centidegrees on public interface to radians
const float heading_rate = radians(heading_rate_cds * 0.01f);
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);
// convert thrust vector to a quaternion attitude
Quaternion thrust_vec_quat = attitude_from_thrust_vector(thrust_vector, 0.0f);
// calculate the angle error in x and y.
float thrust_vector_diff_angle;
Quaternion thrust_vec_correction_quat;
Vector3f attitude_error;
float returned_thrust_vector_angle;
thrust_vector_rotation_angles(thrust_vec_quat, _attitude_target, thrust_vec_correction_quat, attitude_error, returned_thrust_vector_angle, thrust_vector_diff_angle);
if (_rate_bf_ff_enabled) {
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
_ang_vel_target.x = input_shaping_angle(attitude_error.x, _input_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
_ang_vel_target.y = input_shaping_angle(attitude_error.y, _input_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, heading_rate, get_accel_yaw_max_radss(), _dt);
// Limit the angular velocity
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), MIN(radians(_ang_vel_yaw_max), get_slew_yaw_rads()));
} else {
Quaternion yaw_quat;
yaw_quat.from_axis_angle(Vector3f{0.0f, 0.0f, heading_rate * _dt});
_attitude_target = _attitude_target * thrust_vec_correction_quat * yaw_quat;
// Set rate feedforward requests to zero
_euler_rate_target.zero();
_ang_vel_target.zero();
}
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target);
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds)
{
// Convert from centidegrees on public interface to radians
float heading_rate = constrain_float(radians(heading_rate_cds * 0.01f), -get_slew_yaw_rads(), get_slew_yaw_rads());
float heading_angle = radians(heading_angle_cd * 0.01f);
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);
// convert thrust vector and heading to a quaternion attitude
const Quaternion desired_attitude_quat = attitude_from_thrust_vector(thrust_vector, heading_angle);
if (_rate_bf_ff_enabled) {
// calculate the angle error in x and y.
Vector3f attitude_error;
float thrust_vector_diff_angle;
Quaternion thrust_vec_correction_quat;
float returned_thrust_vector_angle;
thrust_vector_rotation_angles(desired_attitude_quat, _attitude_target, thrust_vec_correction_quat, attitude_error, returned_thrust_vector_angle, thrust_vector_diff_angle);
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
_ang_vel_target.x = input_shaping_angle(attitude_error.x, _input_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
_ang_vel_target.y = input_shaping_angle(attitude_error.y, _input_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
_ang_vel_target.z = input_shaping_angle(attitude_error.z, _input_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, heading_rate, get_slew_yaw_rads(), _dt);
// Limit the angular velocity
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), MIN(radians(_ang_vel_yaw_max), get_slew_yaw_rads()));
} else {
// set persisted quaternion target attitude
_attitude_target = desired_attitude_quat;
// Set rate feedforward requests to zero
_euler_rate_target.zero();
_ang_vel_target.zero();
}
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target);
// Call quaternion attitude controller
attitude_controller_run_quat();
}
Quaternion AC_AttitudeControl::attitude_from_thrust_vector(Vector3f thrust_vector, float heading_angle) const
{
const Vector3f thrust_vector_up{0.0f, 0.0f, -1.0f};
if (is_zero(thrust_vector.length_squared())) {
thrust_vector = thrust_vector_up;
} else {
thrust_vector.normalize();
}
// the cross product of the desired and target thrust vector defines the rotation vector
Vector3f thrust_vec_cross = thrust_vector_up % thrust_vector;
// the dot product is used to calculate the angle between the target and desired thrust vectors
const float thrust_vector_angle = acosf(constrain_float(thrust_vector_up * thrust_vector, -1.0f, 1.0f));
// Normalize the thrust rotation vector
const float thrust_vector_length = thrust_vec_cross.length();
if (is_zero(thrust_vector_length) || is_zero(thrust_vector_angle)) {
thrust_vec_cross = thrust_vector_up;
} else {
thrust_vec_cross /= thrust_vector_length;
}
Quaternion thrust_vec_quat;
thrust_vec_quat.from_axis_angle(thrust_vec_cross, thrust_vector_angle);
Quaternion yaw_quat;
yaw_quat.from_axis_angle(Vector3f{0.0f, 0.0f, 1.0f}, heading_angle);
return thrust_vec_quat*yaw_quat;
}
// Calculates the body frame angular velocities to follow the target attitude
void AC_AttitudeControl::attitude_controller_run_quat()
{
@ -639,7 +763,7 @@ void AC_AttitudeControl::attitude_controller_run_quat()
if (_rate_bf_ff_enabled) {
// rotate target and normalize
Quaternion attitude_target_update;
attitude_target_update.from_axis_angle(Vector3f(_ang_vel_target.x * _dt, _ang_vel_target.y * _dt, _ang_vel_target.z * _dt));
attitude_target_update.from_axis_angle(Vector3f{_ang_vel_target.x * _dt, _ang_vel_target.y * _dt, _ang_vel_target.z * _dt});
_attitude_target = _attitude_target * attitude_target_update;
_attitude_target.normalize();
}
@ -653,7 +777,7 @@ void AC_AttitudeControl::attitude_controller_run_quat()
// thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
// The maximum error in the yaw axis is limited based on the angle yaw P value and acceleration.
void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& attitude_target, const Quaternion& attitude_body, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle)
void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& attitude_target, const Quaternion& attitude_body, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle) const
{
Quaternion thrust_vector_correction;
thrust_vector_rotation_angles(attitude_target, attitude_body, thrust_vector_correction, attitude_error, thrust_angle, thrust_error_angle);
@ -666,26 +790,28 @@ void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& attitude_tar
Quaternion yaw_vec_correction_quat;
if (!is_zero(_p_angle_yaw.kP()) && fabsf(attitude_error.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP()) {
attitude_error.z = constrain_float(wrap_PI(attitude_error.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP());
yaw_vec_correction_quat.from_axis_angle(Vector3f(0.0f, 0.0f, attitude_error.z));
yaw_vec_correction_quat.from_axis_angle(Vector3f{0.0f, 0.0f, attitude_error.z});
attitude_target = attitude_body * thrust_vector_correction * yaw_vec_correction_quat;
}
}
// thrust_vector_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
void AC_AttitudeControl::thrust_vector_rotation_angles(const Quaternion& attitude_target, const Quaternion& attitude_body, Quaternion& thrust_vector_correction, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle)
void AC_AttitudeControl::thrust_vector_rotation_angles(const Quaternion& attitude_target, const Quaternion& attitude_body, Quaternion& thrust_vector_correction, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle) const
{
// The direction of thrust is [0,0,-1] is any body-fixed frame, inc. body frame and target frame.
const Vector3f thrust_vector_up{0.0f, 0.0f, -1.0f};
// attitude_target and attitute_body are passive rotations from target / body frames to the NED frame
// Rotating [0,0,-1] by attitude_target expresses (gets a view of) the target thrust vector in the inertial frame
Vector3f att_target_thrust_vec = attitude_target * Vector3f(0.0f, 0.0f, -1.0f); // target thrust vector
Vector3f att_target_thrust_vec = attitude_target * thrust_vector_up; // target thrust vector
// Rotating [0,0,-1] by attitude_target expresses (gets a view of) the current thrust vector in the inertial frame
Vector3f att_body_thrust_vec = attitude_body * Vector3f(0.0f, 0.0f, -1.0f); // current thrust vector
Vector3f att_body_thrust_vec = attitude_body * thrust_vector_up; // current thrust vector
// the dot product is used to calculate the current lean angle for use of external functions
thrust_angle = acosf(constrain_float(Vector3f(0.0f,0.0f,1.0f) * att_body_thrust_vec,-1.0f,1.0f));
thrust_angle = acosf(constrain_float(thrust_vector_up * att_body_thrust_vec,-1.0f,1.0f));
// the cross product of the desired and target thrust vector defines the rotation vector
Vector3f thrust_vec_cross = att_body_thrust_vec % att_target_thrust_vec;
@ -696,7 +822,7 @@ void AC_AttitudeControl::thrust_vector_rotation_angles(const Quaternion& attitud
// Normalize the thrust rotation vector
float thrust_vector_length = thrust_vec_cross.length();
if (is_zero(thrust_vector_length) || is_zero(thrust_error_angle)) {
thrust_vec_cross = Vector3f(0, 0, 1);
thrust_vec_cross = thrust_vector_up;
} else {
thrust_vec_cross /= thrust_vector_length;
}
@ -723,10 +849,13 @@ void AC_AttitudeControl::thrust_vector_rotation_angles(const Quaternion& attitud
// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using _input_tc
float AC_AttitudeControl::input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float dt)
float AC_AttitudeControl::input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float desired_ang_vel, float max_ang_vel, float dt)
{
// Calculate the velocity as error approaches zero with acceleration limited by accel_max_radss
float desired_ang_vel = sqrt_controller(error_angle, 1.0f / MAX(input_tc, 0.01f), accel_max, dt);
desired_ang_vel += sqrt_controller(error_angle, 1.0f / MAX(input_tc, 0.01f), accel_max, dt);
if(is_positive(max_ang_vel)) {
desired_ang_vel = constrain_float(desired_ang_vel, -max_ang_vel, max_ang_vel);
}
// Acceleration is limited directly to smooth the beginning of the curve.
return input_shaping_ang_vel(target_ang_vel, desired_ang_vel, accel_max, dt);
@ -812,7 +941,7 @@ void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
{
float yaw_shift = radians(yaw_shift_cd * 0.01f);
Quaternion _attitude_target_update;
_attitude_target_update.from_axis_angle(Vector3f(0.0f, 0.0f, yaw_shift));
_attitude_target_update.from_axis_angle(Vector3f{0.0f, 0.0f, yaw_shift});
_attitude_target = _attitude_target_update * _attitude_target;
}

15
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -173,6 +173,14 @@ public:
// Command an angular step (i.e change) in body frame angle
virtual void input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd);
// Command a thrust vector in the earth frame and a heading angle and/or rate
void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds);
void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds);
void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_cd) {input_thrust_vector_heading(thrust_vector, heading_cd, 0.0f);}
// Converts thrust vector and heading angle to quaternion rotation in the earth frame
Quaternion attitude_from_thrust_vector(Vector3f thrust_vector, float heading_angle) const;
// Run angular velocity controller and send outputs to the motors
virtual void rate_controller_run() = 0;
@ -283,7 +291,8 @@ public:
// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using smoothing_gain
static float input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel, float dt);
static float input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float desired_ang_vel, float max_ang_vel, float dt);
static float input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel, float dt){ return input_shaping_angle(error_angle, smoothing_gain, accel_max, target_ang_vel, 0.0f, 0.0f, dt); }
// limits the acceleration and deceleration of a velocity request
static float input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt);
@ -303,11 +312,11 @@ public:
// thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
// The maximum error in the yaw axis is limited based on the angle yaw P value and acceleration.
void thrust_heading_rotation_angles(Quaternion& attitude_target, const Quaternion& attitude_body, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle);
void thrust_heading_rotation_angles(Quaternion& attitude_target, const Quaternion& attitude_body, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle) const;
// thrust_vector_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
void thrust_vector_rotation_angles(const Quaternion& attitude_target, const Quaternion& attitude_body, Quaternion& thrust_vector_correction, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle);
void thrust_vector_rotation_angles(const Quaternion& attitude_target, const Quaternion& attitude_body, Quaternion& thrust_vector_correction, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle) const;
// sanity check parameters. should be called once before take-off
virtual void parameter_sanity_check() {}