【ROS&GAZEBO】多旋翼无人机仿真（五）——位置控制器

【ROS&GAZEBO】多旋翼无人机仿真（一）——搭建仿真环境
【ROS&GAZEBO】多旋翼无人机仿真（二）——基于rotors的仿真
【ROS&GAZEBO】多旋翼无人机仿真（三）——自定义多旋翼模型
【ROS&GAZEBO】多旋翼无人机仿真（四）——探索控制器原理
【ROS&GAZEBO】多旋翼无人机仿真（五）——位置控制器

上一篇我们说了rotors中使用了位置控制器节点控制多旋翼的位姿，这一篇我们继续来深入探索位姿控制器的算法

接上篇，我们打开ComputeDesiredAcceleration函数，来看看位置控制究竟是怎样实现的

void LeePositionController::ComputeDesiredAcceleration(Eigen::Vector3d* acceleration) const {
  assert(acceleration);

  Eigen::Vector3d position_error;
  position_error = odometry_.position - command_trajectory_.position_W;

  // Transform velocity to world frame.
  const Eigen::Matrix3d R_W_I = odometry_.orientation.toRotationMatrix();
  Eigen::Vector3d velocity_W =  R_W_I * odometry_.velocity;
  Eigen::Vector3d velocity_error;
  velocity_error = velocity_W - command_trajectory_.velocity_W;

  Eigen::Vector3d e_3(Eigen::Vector3d::UnitZ());

  *acceleration = (position_error.cwiseProduct(controller_parameters_.position_gain_)
      + velocity_error.cwiseProduct(controller_parameters_.velocity_gain_)) / vehicle_parameters_.mass_
      - vehicle_parameters_.gravity_ * e_3 - command_trajectory_.acceleration_W;
}

首先是计算位置误差position_error，这里很好理解，这里注意一点是实际减去期望

  position_error = odometry_.position - command_trajectory_.position_W;

接下来是计算速度误差velocity_error，ROS中的速度是在机体系下表示的，因此需要旋转到世界坐标系
这里const Eigen::Matrix3d R_W_I = odometry_.orientation.toRotationMatrix();的意思是获取旋转矩阵；
Eigen::Vector3d velocity_W = R_W_I * odometry_.velocity;将机体系速度旋转至世界坐标系；
Eigen::Vector3d e_3(Eigen::Vector3d::UnitZ());的意思是定义一个z轴的单位向量，用于将重力转换成向量的表现形式；
*acceleration = (position_error.cwiseProduct(controller_parameters_.position_gain_) + velocity_error.cwiseProduct(controller_parameters_.velocity_gain_)) / vehicle_parameters_.mass_ - vehicle_parameters_.gravity_ * e_3 - command_trajectory_.acceleration_W;将位置误差、速度误差、期望加速度累加起来，作为最终的期望加速度，这里注意到一点是要需要补偿重力加速度，也就是里面的- vehicle_parameters_.gravity_ * e_3这一项。
这里为什么没有使用串级控制，我开始也不理解，我猜想应该是为了能够同时控制位置、速度和加速度才采用这种折中的手段。整个过程用流程图表示如下：

下面可以来测试一下分析的正确性，这里监听的话题是command/trajectory
运行roslaunch rotors_gazebo mav_hovering_example.launch，然后发送话题：

rostopic pub -1 /firefly/command/trajectory trajectory_msgs/MultiDOFJointTrajectory "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
joint_names:
- ''
points:
- transforms:
  - translation:
      x: 0.0
      y: 0.0
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 0.0
  velocities:
  - linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0
  accelerations:
  - linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0
  time_from_start:
    secs: 0
    nsecs: 0" 

在这里去改变速度、位置和加速度，发现四旋翼都能发生移动，说明分析思路是正确的。

rotors中的位置控制就讲解到这里，下一篇将讲解更加核心的姿态控制。

喜欢的朋友可以点个赞，关注微信公众号相互交流：Reed UAV