【ROS&GAZEBO】多旋翼无人机仿真(六)——SE(3)几何姿态控制器

【ROS&GAZEBO】多旋翼无人机仿真(一)——搭建仿真环境https://blog.csdn.net/qq_37680545/article/details/123185002
【ROS&GAZEBO】多旋翼无人机仿真(二)——基于rotors的仿真https://blog.csdn.net/qq_37680545/article/details/123213054
【ROS&GAZEBO】多旋翼无人机仿真(三)——自定义多旋翼模型https://blog.csdn.net/qq_37680545/article/details/123243313

【ROS&GAZEBO】多旋翼无人机仿真(四)——探索控制器原理https://blog.csdn.net/qq_37680545/article/details/123310001
【ROS&GAZEBO】多旋翼无人机仿真(五)——位置控制器https://blog.csdn.net/qq_37680545/article/details/123390601

姿态角控制问题

这一篇我们介绍rotors中的姿态控制算法。

上篇说到,lee_position_controller_node.cpp中包含了位置和姿态控制器,位置控制器我们已经讲过了如何实现,这篇我们来讲姿态控制器是怎样实现的。

在介绍姿态控制之前,我们先直观的来看姿态控制如何实现。按照直观的理解,姿态控制器就是滚转、俯仰、偏航的控制,外环计算误差姿态角,内环计算误差角速度,就可以很好的实现控制。

但是这种方式有一个极大的漏洞——只适用于小角度。为什么这么说呢?因为欧拉角是和旋转顺序有关的,并且存在万向节死锁,这两个bug使得欧拉角控制只能适应小角度下的近似姿态控制。怎么来理解小角度假设呢?

我们来看下面两种旋转顺序:
1)欧拉角定义为3-1-2旋转,(即航向角-俯仰角-滚转角)

\begin{array}{c} {\bf{C}}_n^b = {​{\bf{C}}_\psi }{​{\bf{C}}_\theta }{​{\bf{C}}_\phi }\\ = \left[ {\begin{array}{*{20}{c}} {​{c_\psi }}&{​{s_\psi }}&0\\ { - {s_\psi }}&{​{c_\psi }}&0\\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {​{c_\theta }}&0&{​{s_\theta }}\\ 0&1&0\\ { - {s_\theta }}&0&{​{c_\theta }} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} 1&0&0\\ 0&{​{c_\phi }}&{​{s_\phi }}\\ 0&{ - {s_\phi }}&{​{c_\phi }} \end{array}} \right]\\ = \left[ {\begin{array}{*{20}{c}} {​{c_\theta }{c_\psi }}&{​{c_\phi }{s_\psi } + {c_\psi }{s_\theta }{s_\phi }}&{​{s_\phi }{s_\psi } - {c_\phi }{c_\psi }{s_\theta }}\\ { - {c_\theta }{s_\psi }}&{​{c_\phi }{c_\psi } - {s_\theta }{s_\phi }{s_\psi }}&{​{c_\psi }{s_\phi } + {c_\phi }{s_\theta }{s_\psi }}\\ {​{s_\theta }}&{ - {c_\theta }{s_\phi }}&{​{c_\theta }{c_\phi }} \end{array}} \right] \end{array}


假设旋转角度为小角度:

{c_\psi } = 1,{c_\phi } = 1,{c_\theta } = 1

{s_\psi } = \psi ,{s_\phi } = \phi ,{s_\theta } = \theta ,\psi \theta = 0,\psi \phi = 0,\phi \theta = 0

 对上面的旋转矩阵小角度处理,得到如下结果:

{\bf{C}}_n^b = \left[ {\begin{array}{*{20}{c}} 1&\psi &{ - \theta }\\ \psi &1&\phi \\ \theta &{ - \phi }&1 \end{array}} \right]

2)欧拉角定义为3-2-1旋转,(即航向角-滚转角-俯仰角)

\begin{array}{c} {\bf{C}}_n^b = {​{\bf{C}}_\psi }{​{\bf{C}}_\phi }{​{\bf{C}}_\theta }\\ = \left[ {\begin{array}{*{20}{c}} {​{c_\psi }}&{​{s_\psi }}&0\\ { - {s_\psi }}&{​{c_\psi }}&0\\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} 1&0&0\\ 0&{​{c_\phi }}&{​{s_\phi }}\\ 0&{ - {s_\phi }}&{​{c_\phi }} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {​{c_\theta }}&0&{​{s_\theta }}\\ 0&1&0\\ { - {s_\theta }}&0&{​{c_\theta }} \end{array}} \right]\\ = \left[ {\begin{array}{*{20}{c}} {​{c_\theta }{c_\psi } + {s_\theta }{s_\phi }{s_\psi }}&{​{c_\phi }{s_\psi }}&{​{c_\theta }{s_\psi }{s_\theta } - {c_\psi }{s_\theta }}\\ {​{c_\psi }{s_\theta }{s_\phi } - {c_\theta }{s_\psi }}&{​{c_\phi }{c_\psi }}&{​{s_\psi }{s_\phi } + {s_\phi }{c_\theta }{c_\psi }}\\ {​{s_\theta }{c_\phi }}&{ - {s_\phi }}&{​{c_\theta }{c_\phi }} \end{array}} \right] \end{array}

对上面的旋转矩阵小角度处理,得到如下结果:

{\bf{C}}_n^b = \left[ {\begin{array}{*{20}{c}} 1&\psi &{ - \theta }\\ \psi &1&\phi \\ \theta &{ - \phi }&1 \end{array}} \right]

可以看出,3-1-2和3-2-1两种旋转方式;基于小角度的旋转矩阵是相同的;同时,可以验证出,只要经过三次基本旋转(X、Y、Z轴各转动仅且一次),最终得到的方向余弦矩阵是一样的。但是这些所有结果都是在小角度条件下的。

若不在小角度条件下,不同旋转顺序导致的旋转矩阵结果不相同,这就导致姿态控制存在了问题,姿态控制必须要按旋转顺序进行控制才能回到初始姿态。例如按3-1-2顺序控制,需要先旋转俯仰角、再旋转滚转角、最后再旋转航向角才能回到正确的姿态。而对于飞行器来说,姿态控制频率都非常高,常见的ardupilot、px4等都在1KHz,若按顺序依次进行控制,可能飞机早已经炸机了。

那还有没有能够同时实现三维姿态控制的方法呢?一种就是上面说的小角度假设控制,飞机不进行较大角度的机动,弊端也很明显了,如果想玩花式特技怎么办呢,那结果必然是一次性的表演。。。,第二种方式就是采用四元数控制,这种方法我们留在后面再讲,第三种是几何控制,也就是今天我要介绍的一种控制方法——用姿态矩阵进行控制,目前作者已知就这三种控制方法,不知道会不会有其他更好的方法。

几何控制

几何控制器将多旋翼无人机的动力学全局表示在特殊欧几里德群SE(3)的构型流形上。  该方法已应用于李群上的完全驱动刚体动力学,有全局的渐近稳定性。几何控制器的整体结构如下:

【ROS&GAZEBO】多旋翼无人机仿真(六)——SE(3)几何姿态控制器_第1张图片

平动动力学计算出总的推力f和用于稳定平动动力学的轴向量{\vec b_{3d}},但是这样还差一个自由度航向还未计算出来,因此需要确定航向的轴向量{\vec b_{1d}},再根据航向{\vec b_{1d}}{\vec b_{3d}}{\vec b_{2d}}确定组成一个正交轴系。

 {\vec b_{2d}}怎么确定呢,采用叉乘公式,得到和{\vec b_{1d}}{\vec b_{3d}}正交的{\vec b_{2d}}

 {\vec b_{2d}} = \frac{​{​{​{\vec b}_{3d}} \times {​{\vec b}_{1d}}}}{​{\left\| {​{​{\vec b}_{3d}} \times {​{\vec b}_{1d}}} \right\|}}

这时我们已经获得了{\vec b_{3d}}{\vec b_{2d}}轴了,但是此时{\vec b_{1d}}轴和{\vec b_{3d}}还不是正交的,同样的道理,我们再将{\vec b_{1d}}轴投影到正交轴上:

{\rm{Proj}}\left( {​{​{\vec b}_{1d}}} \right) = {\vec b_{2d}} \times {\vec b_{3d}}

 再重新组合的正交轴系就是我们的期望姿态矩阵{​{\bf{R}}_d}

{​{\bf{R}}_d} = \left[ {​{​{\vec b}_{2d}} \times {​{\vec b}_{2d}},{​{\vec b}_{2d}},{​{\vec b}_{3d}}} \right]

如下图所示:

【ROS&GAZEBO】多旋翼无人机仿真(六)——SE(3)几何姿态控制器_第2张图片

 计算误差姿态:

 根据旋转矩阵计算误差角度公式如下:

 {e_R} = \frac{1}{2}{\left( {R_d^TR - {R^T}{R_d}} \right)^ \vee }

{}_{\vee}代表的意思是从反对称矩阵转换成向量。误差角速度计算如下:

{e_\Omega } = \Omega - {R^T}{R_d}{\Omega _d}

有了误差姿态和角速度,就可以很容易的计算期望力矩了。

M = - {k_R}{​{\bf{e}}_R} - {k_\Omega }{​{\bf{e}}_\Omega } + {\bf{\Omega }} \times {\bf{J\Omega }}

期望力矩由误差姿态、误差角速度、和当前的力矩组成。形式上和期望加速度差不多。

代码:

有了上面的理论分析,现在再回到rotors的代码就很好理解了。对代码的说明已经放在注释里面了。

// Implementation from the T. Lee et al. paper
// Control of complex maneuvers for a quadrotor UAV using geometric methods on SE(3)
void LeePositionController::ComputeDesiredAngularAcc(const Eigen::Vector3d& acceleration,
                                                     Eigen::Vector3d* angular_acceleration) const {
  assert(angular_acceleration);

  Eigen::Matrix3d R = odometry_.orientation.toRotationMatrix();


  //========================================================================
  //计算期望姿态矩阵
  // Get the desired rotation matrix.
  Eigen::Vector3d b1_des;
  double yaw = command_trajectory_.getYaw();
  b1_des << cos(yaw), sin(yaw), 0;

  Eigen::Vector3d b3_des;
  b3_des = -acceleration / acceleration.norm();

  Eigen::Vector3d b2_des;
  b2_des = b3_des.cross(b1_des);
  b2_des.normalize();

  Eigen::Matrix3d R_des;
  R_des.col(0) = b2_des.cross(b3_des);
  R_des.col(1) = b2_des;
  R_des.col(2) = b3_des;

  //========================================================================
  //计算误差角度
  // Angle error according to lee et al.
  Eigen::Matrix3d angle_error_matrix = 0.5 * (R_des.transpose() * R - R.transpose() * R_des);
  Eigen::Vector3d angle_error;
  vectorFromSkewMatrix(angle_error_matrix, &angle_error);

  //========================================================================
  //计算误差角速度
  // TODO(burrimi) include angular rate references at some point.
  Eigen::Vector3d angular_rate_des(Eigen::Vector3d::Zero());
  angular_rate_des[2] = command_trajectory_.getYawRate();

  Eigen::Vector3d angular_rate_error = odometry_.angular_velocity - R_des.transpose() * R * angular_rate_des;

  //========================================================================
  //计算期望角加速度
  *angular_acceleration = -1 * angle_error.cwiseProduct(normalized_attitude_gain_)
                           - angular_rate_error.cwiseProduct(normalized_angular_rate_gain_)
                           + odometry_.angular_velocity.cross(odometry_.angular_velocity); // we don't need the inertia matrix here
}

对rotors的几何控制就分析到这里。

感兴趣的朋友可以点个赞,关注微信公众号交流:Reed UAV
 

【ROS&GAZEBO】多旋翼无人机仿真(六)——SE(3)几何姿态控制器_第3张图片

你可能感兴趣的:(有限位移旋量,无人机仿真,自动化,自动驾驶,人工智能,机器学习)