【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);// 订阅trajectorycmd_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_desR_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角是保持不变

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

【rotors】多旋翼无人机仿真（四）——参数补偿和PID控制
[rotors]多旋翼无人机仿真(一)--搭建rotors仿真环境 [rotors]多旋翼无人机仿真(二)--设置飞行轨迹 [rotors]多旋翼无人机仿真(三)--SE3控制 [rotors]多旋翼 ...
【rotors】多旋翼无人机仿真（二）——设置飞行轨迹
[rotors]多旋翼无人机仿真(一)--搭建rotors仿真环境 [rotors]多旋翼无人机仿真(二)--设置飞行轨迹 [rotors]多旋翼无人机仿真(三)--SE3控制 [rotors]多旋翼 ...
【rotors】多旋翼无人机仿真（一）——搭建rotors仿真环境
[rotors]多旋翼无人机仿真(一)--搭建rotors仿真环境 [rotors]多旋翼无人机仿真(二)--设置飞行轨迹 [rotors]多旋翼无人机仿真(三)--SE3控制 [rotors]多旋翼 ...
多旋翼无人机仿真 rotors_simulator：用键盘控制无人机飞行
多旋翼无人机仿真 rotors_simulator:用键盘控制无人机飞行前言书接上文接口测试键盘指令发布指令转换与发布修改 rotors_simulator 的控制接口节点测试前言 R ...
四旋翼无人机仿真之hector_quadrotor无人机（ROS + Gazebo）（三）传感器数据读取与复现（IMU、GPS）
系列文章目录文章1:四旋翼无人机仿真之hector_quadrotor无人机(ROS + Gazebo) 文章2:四旋翼无人机仿真之hector_quadrotor(二)键盘teleop_twist ...
【ROSGAZEBO】多旋翼无人机仿真（六）——SE(3)几何姿态控制器
[ROS&GAZEBO]多旋翼无人机仿真(一)--搭建仿真环境https://blog.csdn.net/qq_37680545/article/details/123185002 [ROS& ...
多旋翼无人机仿真 rotors_simulator：roll pitch yawrate thrust 控制器
多旋翼无人机仿真 rotors_simulator:roll pitch yawrate thrust 控制器前言 mav_with_keyboard.launch roll_pitch_yawra ...
四旋翼无人机仿真之hector_quadrotor无人机（ROS + Gazebo）（一）
这里写自定义目录标题应用环境 hector_quadrotor 功能包结构简介 hector_quadrotor 功能包安装 1. 安装所需依赖库 2. 下载.安装hector_quadrotor ...
多旋翼无人机仿真 rotors_simulator 是如何悬停的（一）
多旋翼无人机仿真 rotors_simulator 是如何悬停的(一) 前言 mav_hovering_example.launch mav_generic_odometry_sensor.gazeb ...

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