00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include "rotors_control/attitude_controller.h"
00022
00023 AttitudeController::AttitudeController()
00024 : gravity_(9.81),
00025 mass_(1.56779) {
00026 }
00027
00028 AttitudeController::~AttitudeController() {
00029 }
00030
00031 std::shared_ptr<ControllerBase> AttitudeController::Clone() {
00032 std::shared_ptr<ControllerBase> controller(new AttitudeController);
00033
00034 return controller;
00035 }
00036
00037 void AttitudeController::InitializeParams() {
00038 gain_attitude_(0) = 3;
00039 gain_attitude_(1) = 3;
00040 gain_attitude_(2) = 0.035;
00041
00042 gain_angular_rate_(0) = 0.52;
00043 gain_angular_rate_(1) = 0.52;
00044 gain_angular_rate_(2) = 0.025;
00045
00046 amount_rotors_ = 6;
00047 allocation_matrix_.resize(4,amount_rotors_);
00048 allocation_matrix_ << sin(M_PI/6), 1, sin(M_PI/6), -sin(M_PI/6), -1, -sin(M_PI/6),
00049 -cos(M_PI/6), 0, cos(M_PI/6), cos(M_PI/6), 0, -cos(M_PI/6),
00050 -1, 1, -1, 1, -1, 1,
00051 1, 1, 1, 1, 1, 1;
00052
00053 inertia_matrix_<< 0.0347563, 0, 0,
00054 0, 0.0458929, 0,
00055 0, 0, 0.0977;
00056
00057
00058 gain_attitude_ = gain_attitude_.transpose() * inertia_matrix_.inverse();
00059
00060
00061 gain_angular_rate_ = gain_angular_rate_.transpose() * inertia_matrix_.inverse();
00062
00063 const double rotor_force_constant = 0.00000854858;
00064 const double rotor_moment_constant = 0.016;
00065
00066 angular_acc_to_rotor_velocities_.resize(amount_rotors_, 4);
00067 const double arm_length = 0.215;
00068
00069 Eigen::Matrix4d K;
00070 K.setZero();
00071 K(0, 0) = arm_length * rotor_force_constant;
00072 K(1, 1) = arm_length * rotor_force_constant;
00073 K(2, 2) = rotor_force_constant * rotor_moment_constant;
00074 K(3, 3) = rotor_force_constant;
00075
00076 Eigen::Matrix4d I;
00077 I.setZero();
00078 I.block<3, 3>(0, 0) = inertia_matrix_;
00079 I(3, 3) = 1;
00080 angular_acc_to_rotor_velocities_ = allocation_matrix_.transpose()
00081 * (allocation_matrix_ * allocation_matrix_.transpose()).inverse() * K.inverse() * I;
00082 initialized_params_ = true;
00083 }
00084
00085 void AttitudeController::CalculateRotorVelocities(Eigen::VectorXd* rotor_velocities) const {
00086 assert(rotor_velocities);
00087 assert(initialized_params_);
00088
00089 rotor_velocities->resize(amount_rotors_);
00090
00091 Eigen::Vector3d angular_acceleration;
00092 ComputeDesiredAngularAcc(&angular_acceleration);
00093
00094 Eigen::Vector4d angular_acceleration_thrust;
00095 angular_acceleration_thrust.block<3, 1>(0, 0) = angular_acceleration;
00096 angular_acceleration_thrust(3) = control_attitude_thrust_reference_(3);
00097
00098 *rotor_velocities = angular_acc_to_rotor_velocities_ * angular_acceleration_thrust;
00099 *rotor_velocities = rotor_velocities->cwiseMax(Eigen::VectorXd::Zero(rotor_velocities->rows()));
00100 *rotor_velocities = rotor_velocities->cwiseSqrt();
00101 }
00102
00103
00104
00105 void AttitudeController::ComputeDesiredAngularAcc(Eigen::Vector3d* angular_acceleration) const {
00106 assert(angular_acceleration);
00107
00108 Eigen::Matrix3d R = attitude_.toRotationMatrix();
00109
00110
00111 Eigen::Matrix3d R_des;
00112 double yaw = atan2(R(1, 0), R(0, 0));
00113 R_des = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
00114 * Eigen::AngleAxisd(control_attitude_thrust_reference_(0), Eigen::Vector3d::UnitX())
00115 * Eigen::AngleAxisd(control_attitude_thrust_reference_(1), Eigen::Vector3d::UnitY());
00116
00117
00118 Eigen::Matrix3d angle_error_matrix = 0.5 * (R_des.transpose() * R - R.transpose() * R_des);
00119 Eigen::Vector3d angle_error;
00120 angle_error << angle_error_matrix(2, 1),
00121 angle_error_matrix(0, 2), 0;
00122
00123 Eigen::Vector3d angular_rate_des(Eigen::Vector3d::Zero());
00124 angular_rate_des[2] = control_attitude_thrust_reference_(2);
00125
00126 Eigen::Vector3d angular_rate_error = angular_rate_ - R_des.transpose() * R * angular_rate_des;
00127
00128 *angular_acceleration = -1 * angle_error.cwiseProduct(gain_attitude_)
00129 - angular_rate_error.cwiseProduct(gain_angular_rate_)
00130 + angular_rate_.cross(angular_rate_);
00131 }
00132
00133 ROTORS_CONTROL_REGISTER_CONTROLLER(AttitudeController);