From 05e9a30573f50dd271f10864f6e7ec37b6c94043 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Dec 2013 16:21:16 +0400 Subject: [PATCH] multirotor_pos_control & mc_att_control_vector: singularities handling improved --- src/lib/mathlib/math/Vector3.hpp | 1 + .../mc_att_control_vector_main.cpp | 54 ++++++++++++------- .../multirotor_pos_control.c | 52 ++++++++---------- 3 files changed, 56 insertions(+), 51 deletions(-) diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp index 2bf00f26be..1656e184ec 100644 --- a/src/lib/mathlib/math/Vector3.hpp +++ b/src/lib/mathlib/math/Vector3.hpp @@ -56,6 +56,7 @@ public: Vector3 cross(const Vector3 &b) const; Vector3 operator %(const Vector3 &v) const; float operator *(const Vector3 &v) const; + using Vector::operator *; /** * accessors diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 230fa15e40..fb09078faf 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -564,33 +564,39 @@ MulticopterAttitudeControl::task_main() math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - /* sin(angle error) between current and desired thrust vectors (Z axes) */ math::Vector3 R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector3 R_z_des(R_des(0, 2), R_des(1, 2), R_des(2, 2)); - math::Vector3 e_R = R.transpose() * R_z.cross(R_z_des); + math::Vector3 R_des_z(R_des(0, 2), R_des(1, 2), R_des(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector3 e_R = R.transpose() * R_z.cross(R_des_z); /* calculate angle error */ - float e_R_sin = e_R.norm(); - float e_R_cos = R_z * R_z_des; - float angle = atan2f(e_R_sin, e_R_cos); + float e_R_z_sin = e_R.norm(); + float e_R_z_cos = R_z * R_des_z; + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + + /* calculate weight for yaw control */ + float cos_z = cosf(R_z(2)); + float yaw_w = cos_z * cos_z; - /* e_R has zero Z component, set it according to yaw */ /* calculate rotation matrix after roll/pitch only rotation */ math::Matrix R_rp(3, 3); - if (e_R_sin > 0.0f) { + if (e_R_z_sin > 0.0f) { /* get axis-angle representation */ - math::Vector3 e_R_axis = e_R / e_R_sin; + math::Vector3 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 e_R_cp(3, 3); e_R_cp.setAll(0.0f); - e_R_cp(0, 1) = -e_R_axis(2); - e_R_cp(0, 2) = e_R_axis(1); - e_R_cp(1, 0) = e_R_axis(2); - e_R_cp(1, 2) = -e_R_axis(0); - e_R_cp(2, 0) = -e_R_axis(1); - e_R_cp(2, 1) = e_R_axis(0); + 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 */ math::Matrix I(3, 3); @@ -598,7 +604,7 @@ MulticopterAttitudeControl::task_main() I(0, 0) = 1.0f; I(1, 1) = 1.0f; I(2, 2) = 1.0f; - R_rp = R * (I + e_R_cp * e_R_sin + e_R_cp * e_R_cp * (1.0f - e_R_cos)); + 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 */ @@ -606,13 +612,21 @@ MulticopterAttitudeControl::task_main() } /* R_rp and R_des has the same Z axis, calculate yaw error */ - math::Vector3 R_des_z(R_des(0, 2), R_des(1, 2), R_des(2, 2)); math::Vector3 R_des_x(R_des(0, 0), R_des(1, 0), R_des(2, 0)); math::Vector3 R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); - e_R(2) = R_rp_x.cross(R_des_x) * R_des_z; + e_R(2) = atan2f(R_rp_x.cross(R_des_x) * R_des_z, R_rp_x * R_des_x) * yaw_w; - /* don't try to control yaw when thrust vector has opposite direction to desired */ - e_R(2) *= (e_R_cos > 0.0f ? 1.0f : e_R_sin); + if (e_R_z_cos < 0.0) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_des rotation directly */ + math::Quaternion q(R.transpose() * R_des); + float angle_d = 2.0f * atan2f(sqrtf(q.getB() * q.getB() + q.getC() * q.getC() + q.getD() * q.getD()), q.getA()); + math::Vector3 e_R_d(q.getB(), q.getC(), q.getD()); + + /* 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; + } /* angular rates setpoint*/ math::Vector3 rates_sp = _K * e_R; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 7e36872f99..54f9e52b8c 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -71,6 +71,7 @@ #include "thrust_pid.h" #define TILT_COS_MAX 0.7f +#define SIGMA 0.000001f static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -439,7 +440,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ - float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); + float xy_sp_ctl_dir = att_sp.yaw_body + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; @@ -771,7 +772,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { body_z[0] = 0.0f; body_z[1] = 0.0f; - body_z[2] = -1.0f; + body_z[2] = 1.0f; } /* vector of desired yaw direction in XY plane, rotated by PI/2 */ @@ -780,39 +781,28 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) y_C[1] = cosf(att_sp.yaw_body); y_C[2] = 0; - /* desired body_x axis = cross(y_C, body_z) */ - cross3(y_C, body_z, body_x); - float body_x_norm = normalize3(body_x); - static const float body_x_norm_max = 0.5f; + if (fabsf(body_z[2]) < SIGMA) { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x[0] = -body_x[0]; + body_x[1] = -body_x[1]; + body_x[2] = -body_x[2]; + } else { + /* desired body_x axis = cross(y_C, body_z) */ + cross3(y_C, body_z, body_x); + + /* keep nose to front while inverted upside down */ + if (body_z[2] < 0.0f) { + body_x[0] = -body_x[0]; + body_x[1] = -body_x[1]; + body_x[2] = -body_x[2]; + } + normalize3(body_x); + } /* desired body_y axis = cross(body_z, body_x) */ cross3(body_z, body_x, body_y); - if (body_x_norm < body_x_norm_max) { - /* roll is close to +/- PI/2, don't try to hold yaw exactly */ - float x_C[3]; - x_C[0] = cos(att_sp.yaw_body); - x_C[1] = sinf(att_sp.yaw_body); - x_C[2] = 0; - - float body_y_1[3]; - /* desired body_y axis for approximate yaw = cross(body_z, x_C) */ - cross3(body_z, x_C, body_y_1); - - float w = body_x_norm / body_x_norm_max; - float w1 = 1.0f - w; - - /* mix two body_y axes */ - body_y[0] = body_y[0] * w + body_y_1[0] * w1; - body_y[1] = body_y[1] * w + body_y_1[1] * w1; - body_y[2] = body_y[2] * w + body_y_1[2] * w1; - - normalize3(body_y); - - /* desired body_x axis = cross(body_y, body_z) */ - cross3(body_y, body_z, body_x); - } - /* fill rotation matrix */ for (int i = 0; i < 3; i++) { att_sp.R_body[i][0] = body_x[i];