【rotors】多旋翼无人机仿真（三）——SE3控制

【rotors】多旋翼无人机仿真（一）——搭建rotors仿真环境
【rotors】多旋翼无人机仿真（二）——设置飞行轨迹
【rotors】多旋翼无人机仿真（三）——SE3控制
【rotors】多旋翼无人机仿真（四）——参数补偿和PID控制
【rotors】多旋翼无人机仿真（五）——多无人机仿真

本贴内容参考两位博主的内容：
月照银海似蛟
Reed Liao

1、前言

在上一节中，我们分析了hovering_example节点程序，本节中我们来看一下最重要的控制节点lee_position_controller_node。

2、无人机控制

我们先总结一下无人机的动力学内容，其中在控制中用到最多的是欧拉方程和牛顿方程，因为这两个方程给出了如何控制无人机的加速度和角加速度，进而控制无人机的位置和姿态。
无人机控制问题中，我们的状态变量x就是无人机的位置、速度、姿态角、角速度共12个变量，我们的输入u是一个四维向量，隐含着无人机的升力和角加速度。

当然无人机底层控制的输入依旧是电机的转速，因此我们需要将u转换为对应的电机转速，二者之间的系数矩阵是通过欧拉方程和牛顿方程得到的，我们也可以根据系数矩阵看出输入u的含义，那么如果我们可以计算出输入u，只需要乘以系数矩阵的逆再开方就可以得到电机的转速了。

无人机的控制大致可以分为欧拉角控制、旋转矩阵控制、四元数控制，欧拉角控制由于欧拉角本身的缺陷需要小角度控制，旋转矩阵控制的一种方法就是本节要将的SE3，四元数控制可以参考[这个]

3、SE(3)

SE3或者微分平坦控制来之论文：Minimum Snap Trajectory Generation and Control for Quadrotors

公式推到可以参考这个

4、lee_position_controller_node

看完了se3的理论部分，我们来看看rotors是如何代码实现se3控制的，无人机的控制节点是lee_position_controller_node，位置在~/UAV_rotors/src/rotors_simulator/rotors_control/src/nodes文件夹下，lee_position_controller_node.cpp的代码及其解析如下：

/*
 * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <ros/ros.h>
#include <mav_msgs/default_topics.h>

#include "lee_position_controller_node.h"

#include "rotors_control/parameters_ros.h"

namespace rotors_control {
    
    

// 构造函数
LeePositionControllerNode::LeePositionControllerNode(
  const ros::NodeHandle& nh, const ros::NodeHandle& private_nh)
  :nh_(nh),
   private_nh_(private_nh){
    
    
  InitializeParams();
  // 订阅pose话题
  cmd_pose_sub_ = nh_.subscribe(
      mav_msgs::default_topics::COMMAND_POSE, 1,
      &LeePositionControllerNode::CommandPoseCallback, this);
  // 订阅trajectory
  cmd_multi_dof_joint_trajectory_sub_ = nh_.subscribe(
      mav_msgs::default_topics::COMMAND_TRAJECTORY, 1,
      &LeePositionControllerNode::MultiDofJointTrajectoryCallback, this);
  // 订阅Odometry话题
  odometry_sub_ = nh_.subscribe(mav_msgs::default_topics::ODOMETRY, 1,
                               &LeePositionControllerNode::OdometryCallback, this);
  // 发布电机转速话题
  motor_velocity_reference_pub_ = nh_.advertise<mav_msgs::Actuators>(
      mav_msgs::default_topics::COMMAND_ACTUATORS, 1);
  // 创建定时器
  command_timer_ = nh_.createTimer(ros::Duration(0), &LeePositionControllerNode::TimedCommandCallback, this,
                                  true, false);
}

// 析构函数
LeePositionControllerNode::~LeePositionControllerNode() {
    
     }

// 初始化参数
void LeePositionControllerNode::InitializeParams() {
    
    

  // Read parameters from rosparam.
  GetRosParameter(private_nh_, "position_gain/x",
                  lee_position_controller_.controller_parameters_.position_gain_.x(),
                  &lee_position_controller_.controller_parameters_.position_gain_.x());
  GetRosParameter(private_nh_, "position_gain/y",
                  lee_position_controller_.controller_parameters_.position_gain_.y(),
                  &lee_position_controller_.controller_parameters_.position_gain_.y());
  GetRosParameter(private_nh_, "position_gain/z",
                  lee_position_controller_.controller_parameters_.position_gain_.z(),
                  &lee_position_controller_.controller_parameters_.position_gain_.z());
  GetRosParameter(private_nh_, "velocity_gain/x",
                  lee_position_controller_.controller_parameters_.velocity_gain_.x(),
                  &lee_position_controller_.controller_parameters_.velocity_gain_.x());
  GetRosParameter(private_nh_, "velocity_gain/y",
                  lee_position_controller_.controller_parameters_.velocity_gain_.y(),
                  &lee_position_controller_.controller_parameters_.velocity_gain_.y());
  GetRosParameter(private_nh_, "velocity_gain/z",
                  lee_position_controller_.controller_parameters_.velocity_gain_.z(),
                  &lee_position_controller_.controller_parameters_.velocity_gain_.z());
  GetRosParameter(private_nh_, "attitude_gain/x",
                  lee_position_controller_.controller_parameters_.attitude_gain_.x(),
                  &lee_position_controller_.controller_parameters_.attitude_gain_.x());
  GetRosParameter(private_nh_, "attitude_gain/y",
                  lee_position_controller_.controller_parameters_.attitude_gain_.y(),
                  &lee_position_controller_.controller_parameters_.attitude_gain_.y());
  GetRosParameter(private_nh_, "attitude_gain/z",
                  lee_position_controller_.controller_parameters_.attitude_gain_.z(),
                  &lee_position_controller_.controller_parameters_.attitude_gain_.z());
  GetRosParameter(private_nh_, "angular_rate_gain/x",
                  lee_position_controller_.controller_parameters_.angular_rate_gain_.x(),
                  &lee_position_controller_.controller_parameters_.angular_rate_gain_.x());
  GetRosParameter(private_nh_, "angular_rate_gain/y",
                  lee_position_controller_.controller_parameters_.angular_rate_gain_.y(),
                  &lee_position_controller_.controller_parameters_.angular_rate_gain_.y());
  GetRosParameter(private_nh_, "angular_rate_gain/z",
                  lee_position_controller_.controller_parameters_.angular_rate_gain_.z(),
                  &lee_position_controller_.controller_parameters_.angular_rate_gain_.z());
  GetVehicleParameters(private_nh_, &lee_position_controller_.vehicle_parameters_);
  lee_position_controller_.InitializeParameters();
}
void LeePositionControllerNode::Publish() {
    
    
}

// pose话题的回调函数
void LeePositionControllerNode::CommandPoseCallback(
    const geometry_msgs::PoseStampedConstPtr& pose_msg) {
    
    
  // Clear all pending commands.
  command_timer_.stop();
  commands_.clear();
  command_waiting_times_.clear();

  // 将pose信息转换为Trajectory给控制器
  mav_msgs::EigenTrajectoryPoint eigen_reference;
  mav_msgs::eigenTrajectoryPointFromPoseMsg(*pose_msg, &eigen_reference);
  commands_.push_front(eigen_reference);

  lee_position_controller_.SetTrajectoryPoint(commands_.front());
  commands_.pop_front();
}

// Trajectory话题回调函数
void LeePositionControllerNode::MultiDofJointTrajectoryCallback(
    const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& msg) {
    
    
  // Clear all pending commands.
  command_timer_.stop();
  commands_.clear();
  command_waiting_times_.clear();

  const size_t n_commands = msg->points.size();

  if(n_commands < 1){
    
    
    ROS_WARN_STREAM("Got MultiDOFJointTrajectory message, but message has no points.");
    return;
  }

  mav_msgs::EigenTrajectoryPoint eigen_reference;
  mav_msgs::eigenTrajectoryPointFromMsg(msg->points.front(), &eigen_reference);
  commands_.push_front(eigen_reference);

  for (size_t i = 1; i < n_commands; ++i) {
    
    
    const trajectory_msgs::MultiDOFJointTrajectoryPoint& reference_before = msg->points[i-1];
    const trajectory_msgs::MultiDOFJointTrajectoryPoint& current_reference = msg->points[i];

    mav_msgs::eigenTrajectoryPointFromMsg(current_reference, &eigen_reference);
    // 如果是多个Trajectory就依次送入到队列中
    commands_.push_back(eigen_reference);
    // command_waiting_times_记录了每个Trajectory之间相隔的时间
    command_waiting_times_.push_back(current_reference.time_from_start - reference_before.time_from_start);
  }

  // We can trigger the first command immediately.
  // 将第一条命令直接发送给控制器
  lee_position_controller_.SetTrajectoryPoint(commands_.front());
  commands_.pop_front();

  if (n_commands > 1) {
    
    
    // 如果控制命令多余1个,就在定时器中逐一传入控制命令
    command_timer_.setPeriod(command_waiting_times_.front());
    command_waiting_times_.pop_front();
    command_timer_.start();
  }
}

// 定时器回调函数
void LeePositionControllerNode::TimedCommandCallback(const ros::TimerEvent& e) {
    
    

  if(commands_.empty()){
    
    
    ROS_WARN("Commands empty, this should not happen here");
    return;
  }

  const mav_msgs::EigenTrajectoryPoint eigen_reference = commands_.front();
  // 传入控制指令
  lee_position_controller_.SetTrajectoryPoint(commands_.front());
  commands_.pop_front();
  command_timer_.stop();
  if(!command_waiting_times_.empty()){
    
    
    // 如果还有控制指令就重新启动一次定时器,一次定时器输入一次控制指令
    command_timer_.setPeriod(command_waiting_times_.front());
    command_waiting_times_.pop_front();
    command_timer_.start();
  }
}

// Odometry话题的回调函数
void LeePositionControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) {
    
    

  ROS_INFO_ONCE("LeePositionController got first odometry message.");

  // 接受到一次Odometry相当于一次负反馈循环
  EigenOdometry odometry;
  eigenOdometryFromMsg(odometry_msg, &odometry);
  lee_position_controller_.SetOdometry(odometry);

  // 根据负反馈结果可以计算出循环的电压输出,因此循环的频率取决于Odometry消息的发送速度
  Eigen::VectorXd ref_rotor_velocities;
  lee_position_controller_.CalculateRotorVelocities(&ref_rotor_velocities);

  // Todo(ffurrer): Do this in the conversions header.
  mav_msgs::ActuatorsPtr actuator_msg(new mav_msgs::Actuators);

  actuator_msg->angular_velocities.clear();
  for (int i = 0; i < ref_rotor_velocities.size(); i++)
    actuator_msg->angular_velocities.push_back(ref_rotor_velocities[i]);
  actuator_msg->header.stamp = odometry_msg->header.stamp;

  motor_velocity_reference_pub_.publish(actuator_msg);
}

}

int main(int argc, char** argv) {
    
    
  ros::init(argc, argv, "lee_position_controller_node");

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");
  rotors_control::LeePositionControllerNode lee_position_controller_node(nh, private_nh);

  ros::spin();

  return 0;
}

lee_position_controller_node是经过封装的控制节点，内部真正的控制器是lee_position_controller_ ，lee_position_controller_node 订阅了trajectory话题、Odometry话题、电机转速话题，当接收到trajectory消息或者Odometry消息时就把消息内容传给控制器lee_position_controller_ ，最后控制器会根据接收到的话题消息计算出相应的电机转速，然后通过电机转速话题将期望电机转速发布出去控制无人机飞行，此外还创建了一个定时器，因为lee_position_controller_一次只能传一个trajectory，当接收到多个trajectory时，会设置定时器计时时间，然后定时器在计时时间到了后会执行定时器回调函数，在回调函数中传入下一个trajectory，不断回调执行直到队列中无等待的trajectory

5、lee_position_controller

我们进一步看一下控制器lee_position_controller_，它是LeePositionController类的实例，LeePositionController类的位置在~/UAV_rotors/src/rotors_simulator/rotors_control/src/library/lee_position_controller.cpp，我们具体看一下lee_position_controller.cpp的内容，代码及其注释如下（下面分函数解析）

5.1 LeePositionController::InitializeParameters()

// 一些无人机参数的初始化
void LeePositionController::InitializeParameters() {
    
    
  calculateAllocationMatrix(vehicle_parameters_.rotor_configuration_, &(controller_parameters_.allocation_matrix_));
  // To make the tuning independent of the inertia matrix we divide here.
  normalized_attitude_gain_ = controller_parameters_.attitude_gain_.transpose()
      * vehicle_parameters_.inertia_.inverse();
  // To make the tuning independent of the inertia matrix we divide here.
  normalized_angular_rate_gain_ = controller_parameters_.angular_rate_gain_.transpose()
      * vehicle_parameters_.inertia_.inverse();

  Eigen::Matrix4d I;
  I.setZero();
  I.block<3, 3>(0, 0) = vehicle_parameters_.inertia_;
  I(3, 3) = 1;
  angular_acc_to_rotor_velocities_.resize(vehicle_parameters_.rotor_configuration_.rotors.size(), 4);
  // Calculate the pseude-inverse A^{ \dagger} and then multiply by the inertia matrix I.
  // A^{ \dagger} = A^T*(A*A^T)^{-1}
  // 这里angular_acc_to_rotor_velocities_参数比较重要
  angular_acc_to_rotor_velocities_ = controller_parameters_.allocation_matrix_.transpose()
      * (controller_parameters_.allocation_matrix_
      * controller_parameters_.allocation_matrix_.transpose()).inverse() * I;
  initialized_params_ = true;
}

angular_acc_to_rotor_velocities_矩阵就是系数矩阵controller_parameters_.allocation_matrix_的右伪逆：A^-1=A^T (AA^T)-1

之所以使用伪逆是怕系数矩阵不存在逆

5.2 LeePositionController::CalculateRotorVelocities()

// 计算期望电机转速，函数输入是电机的期望转速
void LeePositionController::CalculateRotorVelocities(Eigen::VectorXd* rotor_velocities) const {
    
    
  assert(rotor_velocities);
  assert(initialized_params_);

  rotor_velocities->resize(vehicle_parameters_.rotor_configuration_.rotors.size());
  // Return 0 velocities on all rotors, until the first command is received.
  // 如果控制器还为激活就输出0转速
  if (!controller_active_) {
    
    
    *rotor_velocities = Eigen::VectorXd::Zero(rotor_velocities->rows());
    return;
  }	
  // acceleration是期望加速度
  Eigen::Vector3d acceleration;
  ComputeDesiredAcceleration(&acceleration);
	// angular_acceleration是期望角速度[u2 u3 u4]
  Eigen::Vector3d angular_acceleration;
  ComputeDesiredAngularAcc(acceleration, &angular_acceleration);

  // Project thrust onto body z axis.
  // 计算期望升力u1，实现升力是加速度乘以质量，但是无人机升力方向只是向上，因此还要点乘当前无人机z轴方向向量
  double thrust = -vehicle_parameters_.mass_ * acceleration.dot(odometry_.orientation.toRotationMatrix().col(2));

  Eigen::Vector4d angular_acceleration_thrust;
  angular_acceleration_thrust.block<3, 1>(0, 0) = angular_acceleration;
  angular_acceleration_thrust(3) = thrust;
	// 计算出u了，只要u右乘以系数矩阵的逆就可以得到电机转速的平方
  *rotor_velocities = angular_acc_to_rotor_velocities_ * angular_acceleration_thrust;
    // Eigen库的cwise函数就是按元素操作，这里cwiseMax的操作就是两个向量取最大，也就是电机转速平方限制最小是0
  *rotor_velocities = rotor_velocities->cwiseMax(Eigen::VectorXd::Zero(rotor_velocities->rows()));
    // 将电机转速的平方 开平方得到期望的电机转速
  *rotor_velocities = rotor_velocities->cwiseSqrt();
}

总结一下，这里是操作就是计算期望输入u = [u1 u2 u3 u4]，然后把u转换为电机转速：

5.3 LeePositionController::SetOdometry()

// 设置Odometry里程计
void LeePositionController::SetOdometry(const EigenOdometry& odometry) {
    
    
  odometry_ = odometry;
}
// 设置TrajectoryPoint 无人机目标点(x y z yaw)
void LeePositionController::SetTrajectoryPoint(
    const mav_msgs::EigenTrajectoryPoint& command_trajectory) {
    
    
  command_trajectory_ = command_trajectory;
  controller_active_ = true; // 激活控制器
}

5.4 LeePositionController::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; // 里程计里面的速度是body坐标系的，这里转换为世界坐标系
  Eigen::Vector3d velocity_error;
    // 速度误差
  velocity_error = velocity_W - command_trajectory_.velocity_W;
	// 世界坐标系的z轴方向向量
  Eigen::Vector3d e_3(Eigen::Vector3d::UnitZ());
	// 计算期望加速度，先算出期望升力F，然后除以质量m
  *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;
}

计算期望加速度，先算出期望升力F，然后除以质量m，总结就是：

5.5 LeePositionController::ComputeDesiredAngularAcc()

// 计算期望角加速度[u2 u3 u4]，输入是期望加速度和角加速度
// 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(); //旋转矩阵R

  // Get the desired rotation matrix.
  Eigen::Vector3d b1_des;
  double yaw = command_trajectory_.getYaw();
  b1_des << cos(yaw), sin(yaw), 0;  // b1_des就是Xc轴方向向量

  Eigen::Vector3d b3_des;
  b3_des = -acceleration / acceleration.norm(); // b3_des就是Zb轴方向向量

  Eigen::Vector3d b2_des;
  b2_des = b3_des.cross(b1_des); // b2_des就是Yb轴方向向量
  b2_des.normalize();

  Eigen::Matrix3d R_des; // 期望的旋转矩阵R_des
  R_des.col(0) = b2_des.cross(b3_des); // Xb轴方向向量
  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); //从矩阵得到叉乘向量，就是公式里面的v操作
    

  // 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
}
}

总结就是：

总得来说，lee_position_controller控制器，需要传入里程计消息和目标轨迹消息，然后使用se3方法计算出期望电机转速

6、roll_pitch_yawrate_thrust_controller_node

在~/UAV_rotors/src/rotors_simulator/rotors_control/src/nodes文件夹下还有一个roll_pitch_yawrate_thrust_controller_node控制节点，该控制节点也是使用se3控制，代码和lee_position_controller_node相近，这里就不做多解释了，说一下这两个控制节点的区别吧，lee_position_controller_node控制节点的轨迹消息包括(x y z yaw)，而roll_pitch_yawrate_thrust_controller_node的轨迹消息包括(推力大小 、roll、pitch、yawrate)，这是一个给无人机遥控器开发的控制节点，因为遥控器控制无人机时给出的指令就是推力大小、roll角度、pitch角度、yawrate，这里三个姿态角只有yaw角是角速度，因为遥控松开时roll和pitch是回归0度，而yaw角是保持不变