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

ReedUAV

已于 2022-03-12 18:35:36 修改

阅读量4.3k

点赞数 12

分类专栏：有限位移旋量无人机仿真自动化文章标签：自动驾驶人工智能机器学习

于 2022-03-12 18:27:18 首次发布

本文链接：https://blog.csdn.net/qq_37680545/article/details/123446456

版权

自动化同时被 3 个专栏收录

11 篇文章 19 订阅

订阅专栏

无人机仿真

9 篇文章 27 订阅

订阅专栏

有限位移旋量

3 篇文章 8 订阅

订阅专栏

【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)的构型流形上。该方法已应用于李群上的完全驱动刚体动力学，有全局的渐近稳定性。几何控制器的整体结构如下：

平动动力学计算出总的推力 $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]$

如下图所示：

计算误差姿态：

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

${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

ReedUAV

关注

12
点赞
踩
60

收藏

觉得还不错? 一键收藏
2
评论
【ROS&GAZEBO】多旋翼无人机仿真（六）——SE(3)几何姿态控制器

【ROS&GAZEBO】多旋翼无人机仿真（一）——搭建仿真环境【ROS&GAZEBO】多旋翼无人机仿真（二）——基于rotors的仿真【ROS&GAZEBO】多旋翼无人机仿真（三）——自定义多旋翼模型【ROS&GAZEBO】多旋翼无人机仿真（四）——探索控制器原理【ROS&GAZEBO】多旋翼无人机仿真（五）——位置控制器【ROS&GAZEBO】多旋翼无人机仿真（六）——SE(3)几何姿态控制器姿态角控制问题这一篇我们介绍rotors中的
复制链接

扫一扫