diff --git a/src/lib/matrix b/src/lib/matrix index e595ebb9a7..f4243160e2 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit e595ebb9a704c24aeae990dac768d26949fcaee0 +Subproject commit f4243160e2c77eb8034a35fe42924d59a39319da diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 0eb5c65d6d..c9aa58e797 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -35,9 +35,10 @@ * @file mc_att_control_main.cpp * Multicopter attitude controller. * - * Publication for the desired attitude tracking: - * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. - * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011. + * Publication documenting this type of Quaternion Attitude Control: + * Nonlinear Quadrocopter Attitude Control (2013) + * by Dario Brescianini, Markus Hehn and Raffaello D’Andrea + * Institute for Dynamic Systems and Control (IDSC), ETH Zurich * * @author Lorenz Meier * @author Anton Babushkin @@ -835,84 +836,26 @@ void MulticopterAttitudeControl::control_attitude(float dt) { vehicle_attitude_setpoint_poll(); - _thrust_sp = _v_att_sp.thrust; - /* construct attitude setpoint rotation matrix */ - math::Quaternion q_sp(_v_att_sp.q_d[0], _v_att_sp.q_d[1], _v_att_sp.q_d[2], _v_att_sp.q_d[3]); - math::Matrix<3, 3> R_sp = q_sp.to_dcm(); + using namespace matrix; + float yaw_w = .4f; - /* get current rotation matrix from control state quaternions */ - math::Quaternion q_att(_v_att.q[0], _v_att.q[1], _v_att.q[2], _v_att.q[3]); - math::Matrix<3, 3> R = q_att.to_dcm(); + /* get estimated and desired vehicle attitude */ + Quatf q(_v_att.q); + Quatf qd(_v_att_sp.q_d); - /* all input data is ready, run controller itself */ + /* ensure quaternions are exactly normalized because acosf(1.00001) == NaN */ + q.normalize(); + qd.normalize(); - /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + /* full quaternion attitude control, qe is rotation from q to qd */ + Quatf qe = q.inversed() * qd; - /* axis and sin(angle) of desired rotation (indexes: 0=pitch, 1=roll, 2=yaw). - * This is for roll/pitch only (tilt), e_R(2) is 0 */ - math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); - - /* calculate angle error */ - float e_R_z_sin = e_R.length(); // == sin(tilt angle error) - float e_R_z_cos = R_z * R_sp_z; // == cos(tilt angle error) == (R.transposed() * R_sp)(2, 2) - - /* calculate rotation matrix after roll/pitch only rotation */ - math::Matrix<3, 3> R_rp; - - if (e_R_z_sin > 0.0f) { - /* get axis-angle representation */ - float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; - - e_R = e_R_z_axis * e_R_z_angle; - - /* cross product matrix for e_R_axis */ - math::Matrix<3, 3> e_R_cp; - e_R_cp.zero(); - e_R_cp(0, 1) = -e_R_z_axis(2); - e_R_cp(0, 2) = e_R_z_axis(1); - e_R_cp(1, 0) = e_R_z_axis(2); - e_R_cp(1, 2) = -e_R_z_axis(0); - e_R_cp(2, 0) = -e_R_z_axis(1); - e_R_cp(2, 1) = e_R_z_axis(0); - - /* rotation matrix for roll/pitch only rotation */ - R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); - - } else { - /* zero roll/pitch rotation */ - R_rp = R; - } - - /* R_rp and R_sp have the same Z axis, calculate yaw error */ - math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); - math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); - - /* calculate the weight for yaw control - * Make the weight depend on the tilt angle error: the higher the error of roll and/or pitch, the lower - * the weight that we use to control the yaw. This gives precedence to roll & pitch correction. - * The weight is 1 if there is no tilt error. - */ - float yaw_w = e_R_z_cos * e_R_z_cos; - - /* calculate the angle between R_rp_x and R_sp_x (yaw angle error), and apply the yaw weight */ - e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; - - if (e_R_z_cos < 0.0f) { - /* for large thrust vector rotations use another rotation method: - * calculate angle and axis for R -> R_sp rotation directly */ - math::Quaternion q_error; - q_error.from_dcm(R.transposed() * R_sp); - math::Vector<3> e_R_d = q_error(0) >= 0.0f ? q_error.imag() * 2.0f : -q_error.imag() * 2.0f; - - /* use fusion of Z axis based rotation and direct rotation */ - float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; - e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; - } + /* using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) + * also taking care of the antipodal unit quaternion ambiguity */ + Vector3f eq = 2.f * math::sign(qe(0)) * qe.imag(); + math::Vector<3> e_R(eq.data()); /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); @@ -922,11 +865,11 @@ MulticopterAttitudeControl::control_attitude(float dt) * The following is a simplification of: * R.transposed() * math::Vector<3>(0.f, 0.f, _v_att_sp.yaw_sp_move_rate * _params.yaw_ff) */ - math::Vector<3> yaw_feedforward_rate(R(2, 0), R(2, 1), R(2, 2)); + Vector3f yaw_feedforward_rate = q.inversed().dcm_z(); yaw_feedforward_rate *= _v_att_sp.yaw_sp_move_rate * _params.yaw_ff; yaw_feedforward_rate(2) *= yaw_w; - _rates_sp += yaw_feedforward_rate; + _rates_sp += math::Vector<3>(yaw_feedforward_rate.data()); /* limit rates */