How to control the heading of a drone?

[EdXian]

Hi @Jaeyoung-Lim , This package helps me a lot. Thank you for your package. I have a question about the heading problem with the geometric control. According to the reference, the heading of a drone can be choosed by the user. However, I can not find the function to set mavYaw_.The default mavYaw_ is 0.

[Jaeyoung-Lim]

@EdXian Thank you for the kind words,

Indeed, this is not properly implemented yet. I have a pending PR: https://github.com/Jaeyoung-Lim/mavros_controllers/pull/35 which I tried to make the drone keep the heading in the direction of velocity, but is not finished yet.

Do you simply want to set the heading of the vehicle statically? this can be easily implemented. Dynamically, there needs to be some change in the atttitude controller as large step inputs in yaw commands can create instabilities of the system

[EdXian]

Hi @Jaeyoung-Lim , Thanks for your answer. I have another question about the function acc2quaternion here It seems that the order of step is wrong. Maybe, the snippet should like as follows:

 yc = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())*Eigen::Vector3d::UnitY();
  zb_des = vector_acc / vector_acc.norm();
  xb_des = yc.cross(zb_des) / ( yc.cross(zb_des) ).norm();
  yb_des = zb_des.cross(xb_des) / (zb_des.cross(xb_des)).norm();

  rotmat << xb_des(0), yb_des(0), zb_des(0),
            xb_des(1), yb_des(1), zb_des(1),
            xb_des(2), yb_des(2), zb_des(2);
  quat = rot2Quaternion(rotmat);

[Jaeyoung-Lim]

@EdXian I think you are right. Good catch!

Would it be possible if you make a PR on this? I would also like to check if this is also true on our forumulation as we are not using the NED frame as written in the paper.

This could also explain some wierd behaviors I was observing...

[EdXian]

Hi @Jaeyoung-Lim, I will also double check for it. If it is correct, I will make a PR for this issue. Cheers!

[Jaeyoung-Lim]

@EdXian Do you have a ETA on this? I would like to rebase #35 on your PR

[EdXian]

Hi @Jaeyoung-Lim, Sorry for that I only changed a part of code in acc2quaternion. I still think about how to implement a control law for heading control . I will talk about this issue with my cowoker this week.

By the way, I read the reference again yesterday. I found the essential of the geometric control is controlling force and moment in body frame.(Maybe I misunderstood this part). I think the control input for the control loop is force. Then I assume the relationship between force and thrust can be represented by a quadratic equation (f = at^2+bt+c). Finally, I tried to find a b c by least square method. In our case, a,b and c can be 20, 15 and 0.

As you can see in the following figure, the red line is the total force exerted by four motors while the blue line is calculated by (f = at^2+bt+c). (I used virtual F/T sensor in gazebo directly.)

Then I tried to modified the code as following snippets.

void geometricCtrl::computeBodyRateCmd(bool ctrl_mode){
double mass =1.5;   // The mass of IRIS is 1.5 kilogram.
  if(ctrl_mode){      
    ...
    a_des = mass* a_ref + a_fb  - mass * g_;     
    q_des = acc2quaternion(a_des, mavYaw_);
      ...

  } else {
    ...
    a_des = mass* a_ref + a_fb  - mass * g_;
    q_des = acc2quaternion(a_des, mavYaw_);
    ...
  }
  cmdBodyRate_ = attcontroller(q_des, a_des, mavAtt_); //Calculate BodyRate
}

 Eigen::Vector4d geometricCtrl::attcontroller(Eigen::Vector4d &ref_att, Eigen::Vector3d &ref_acc, 
 Eigen::Vector4d &curr_att){
  Eigen::Vector4d ratecmd;
  Eigen::Vector4d qe, q_inv, inverse;
  Eigen::Matrix3d rotmat;
  Eigen::Vector3d zb;
  inverse << 1.0, -1.0, -1.0, -1.0;

  q_inv = inverse.asDiagonal() * curr_att;
  qe = quatMultiplication(q_inv, ref_att);
  ratecmd(0) = (2.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(1);
  ratecmd(1) = (2.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(2);
  ratecmd(2) = (2.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(3);

  rotmat = quat2RotMatrix(mavAtt_);
  zb = rotmat.col(2);

  //The desired thrust should be in the interval [0,1].
  //ratecmd(3) = std::max(0.0, std::min(1.0, norm_thrust_const_ * ref_acc.dot(zb))); //Calculate thrust

  //solve a quadratic equation  20T^2+15T^2-f = 0 where T is the thrust of drone.
  //The coefficient of the quadratic equation is determined by RLS method.

  double thrust = ref_acc.dot(zb);
  double a=20,b=15,c = -1.0000000*thrust;
  double x =0;

  std::vector<double> root;
  if((b*b - 4*a*c)>0){

      double sqrt_ =  sqrt(b*b - 4.00000*a*c);
      root.push_back(  (    -1.0000000 * b+     sqrt_) /(2.0000000*a));
      root.push_back(  (    -1.0000000 * b-     sqrt_) /(2.0000000*a));
  }

    for(unsigned int i=0;i<root.size();i++){
        if(root[i]>0){
          ratecmd(3) = root[i];                  // The thrust cmd should be greater than 0.
        }
    }

  return ratecmd;

}

Maybe It can make the simulation more closed to a real scenario.

Good Luck!!

[Jaeyoung-Lim]

@EdXian You are right, that the geometric control assumes controlling the drone using force / moment.

However, this is quite hard to use in real platforms as your force estimation will be always off as most drones use non-feedback ESCs to drive the motors. I have used force inputs on real platforms and I end up using a lot of time to tune the system- and you need to tune it for every system. The delay caused by mavros also introduces some time delay problems depending on your hardware implementations

Though, your plots seem quite impressive, and I would be very excited to have this functionality in this repo as it will be cool if we can evaluate what effects it has. If you are interested on implementing this (as you already have), I would love to have this merged in master too.

For the geometric_controller in this repo, I tried to solve this problem on using the bodyrate commands so that PX4 running on the flight controller will be able to compensate the effects by its onboard bodyrate feedback controller. This is neat in the sense that it can be applied to arbitrary sytems that you have no knowledge of the inertial properties. It is also a valid assumption on quadrotor systems to assume when you command a bodyrate command, the bodyrate changes instantly, as the angular accelration limits are usually quite high (for small drones)

That being said the geometric_controller in this repo actually only uses the attitude reference that is calculated from the geometric controller in Lee(2010) and uses a different attitude controller which is closer to this paper from ETH: https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf

I have already have a implementation of the heading tracking with a reduced attitude controller in #35 but needs some debugging. I can try and invest some time on this if you need it

[EdXian]

Hi, Thank you for your suggestion!! Yes, the controller in this package is more suited for general case than the formal geometric control. I have read the techanical report. The difference between the geometric control and reduced/full attitude control is that the former using rotation matrix (R_3x3) to calculate the desired angular rate while the latter using quarternion error.
I think the geometric control and reduced/ full attitude control has the same effect on a drone. Have you ever analyzed this two kinds of method before? Good Luck!!

[Jaeyoung-Lim]

@EdXian So yes, the difference is that the two controllers define the attitude error with a different error function.

I haven't done any benchmarks between the two methods. It will be extremely interesting to do so.

I like the error quaternion approach as the imaginary axis of the quaternion is the axis of shortest rotation path between the two attitudes. I think it is quite smart to use this as a angular velocty rate command.

[EdXian]

Hi @Jaeyoung-Lim ,

I like the error quaternion approach as the imaginary axis of the quaternion is the axis of shortest rotation path between the two attitudes. I think it is quite smart to use this as a angular velocty rate command.

May I ask some question that is not related to this issue? In the conclusion of this paper, the author has mentioned that the Eucledian-based method can avoid the ambiguities of quaternions in representing attitude. Is it imply that some issue exsists in quaternion based attitude controller? Then, why do we use quarternion-based method rather than Eucledian-based method ? Thanks!

[Jaeyoung-Lim]

@EdXian The quaternion based controller can be found here

The answer is no. The quaternion controller is also asymptotically stable in the whole SO(3). The ambiguity is refering to that the quaternion is a redundent representation. Two quaternions can represent the same rotation. However, as far as I understand, this is not a problem if we use the quaternion error based controller

I used the quaternion simply because it is a much simpler and intuitive error metric which also involves less computation.

If you are interested in comparing the two, I would be extremely excited to compare the results and even have it merged to this repo.

[Jaeyoung-Lim]

@EdXian This might be useful for you : https://github.com/fdcl-gwu/se3_geometric_control

[EdXian]

Hi @Jaeyoung-Lim , Thank you for your decent information. The repository above is very helpful for me.