attitude_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00007  *
00008  * Licensed under the Apache License, Version 2.0 (the "License");
00009  * you may not use this file except in compliance with the License.
00010  * You may obtain a copy of the License at
00011  *
00012  *     http://www.apache.org/licenses/LICENSE-2.0
00013 
00014  * Unless required by applicable law or agreed to in writing, software
00015  * distributed under the License is distributed on an "AS IS" BASIS,
00016  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00017  * See the License for the specific language governing permissions and
00018  * limitations under the License.
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; //4
00039   gain_attitude_(1) = 3; //4
00040   gain_attitude_(2) = 0.035;
00041 
00042   gain_angular_rate_(0) = 0.52;//0.6;
00043   gain_angular_rate_(1) = 0.52;//0.6;
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   // to make the tuning independent of the inertia matrix we divide here
00058   gain_attitude_ = gain_attitude_.transpose() * inertia_matrix_.inverse();
00059 
00060   // to make the tuning independent of the inertia matrix we divide here
00061   gain_angular_rate_ = gain_angular_rate_.transpose() * inertia_matrix_.inverse();
00062 
00063   const double rotor_force_constant = 0.00000854858;  //F_i = k_n * rotor_velocity_i^2
00064   const double rotor_moment_constant = 0.016;  // M_i = k_m * F_i
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 // Implementation from the T. Lee et al. paper
00104 // Control of complex maneuvers for a quadrotor UAV using geometric methods on SE(3)
00105 void AttitudeController::ComputeDesiredAngularAcc(Eigen::Vector3d* angular_acceleration) const {
00106   assert(angular_acceleration);
00107 
00108   Eigen::Matrix3d R = attitude_.toRotationMatrix();
00109 
00110   // get desired rotation matrix
00111   Eigen::Matrix3d R_des;
00112   double yaw = atan2(R(1, 0), R(0, 0));
00113   R_des = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())  // yaw
00114         * Eigen::AngleAxisd(control_attitude_thrust_reference_(0), Eigen::Vector3d::UnitX())  // roll
00115         * Eigen::AngleAxisd(control_attitude_thrust_reference_(1), Eigen::Vector3d::UnitY());  // pitch
00116 
00117       // angle error according to lee et al.
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),  // inverse skew operator
00121   angle_error_matrix(0, 2), 0;  // angle_error_matrix(1,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_); // we don't need the inertia matrix here
00131 }
00132 
00133 ROTORS_CONTROL_REGISTER_CONTROLLER(AttitudeController);


rotors_control
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:38