【rotors】Multi-rotor UAV simulation (1) - Build a rotors simulation environment
【rotors】Multi-rotor UAV simulation (2) - set flight trajectory
【rotors】Multi-rotor UAV simulation (3) - SE3 Control
[rotors] multi-rotor UAV simulation (four) - parameter compensation and PID control
[rotors] multi-rotor UAV simulation (five) - multi-UAV simulation
The content of this post refers to the content of two bloggers: Reed
Liao
1 Introduction
In the previous section , we analyzed hovering_examplethe node program, in this section we look at the most important control nodes lee_position_controller_node.
2. UAV control
Let’s summarize the dynamics of the UAV first. Euler’s equation and Newton’s equation are the most used in control, because these two equations give how to control the acceleration and angular acceleration of the UAV, and then control the UAV. The position and attitude of the human-machine.
In the UAV control problem, our state variable x is 12 variables including the UAV's position, velocity, attitude angle, and angular velocity. Our input u is a four-dimensional vector, which implies the UAV's lift and angular acceleration. .
Of course, the input of the drone's underlying control is still the speed of the motor, so we need to convert u into the corresponding motor speed. The coefficient matrix between the two is obtained through Euler's equation and Newton's equation. We can also use the coefficient matrix See the meaning of the input u, then if we can calculate the input u, we only need to multiply by the inverse of the coefficient matrix and then take the square root to get the speed of the motor.
The control of UAVs can be roughly divided into Euler angle control, rotation matrix control, and quaternion control. Euler angle control requires small angle control due to the defects of Euler angle itself. One method of rotation matrix control is this section. For SE3, quaternion control can refer to [this]
3、SE(3)
SE3 or differential flat control paper: Minimum Snap Trajectory Generation and Control for Quadrotors
The formula can be pushed to refer to this
4、lee_position_controller_node
After reading the theoretical part of se3, let's take a look at how the rotors code realizes the control of se3. The control node of the drone is lee_position_controller_node, and the location is under the ~/UAV_rotors/src/rotors_simulator/rotors_control/src/nodesfolder. lee_position_controller_node.cppThe code and its analysis are as follows:
/*
* 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 is an encapsulated control node. The real controller inside is that it lee_position_controller_subscribes lee_position_controller_nodeto the topic of trajectory, odometry, and motor speed lee_position_controller_. The received topic message calculates the corresponding motor speed, and then publishes the expected motor speed through the topic of motor speed to control the flight of the drone. In addition, a timer is created, because only one trajectory can be transmitted at a time. When lee_position_controller_multiple In the case of trajectory, the timer timing time will be set, and then the timer will execute the timer callback function after the timing time is up, pass in the next trajectory in the callback function, and continue to call back until there is no waiting trajectory in the queue
5、lee_position_controller
Let's take a closer look at the controller lee_position_controller_, which is LeePositionControlleran instance of the class, LeePositionControllerand the position of the class is here ~/UAV_rotors/src/rotors_simulator/rotors_control/src/library/lee_position_controller.cpp. Let's take a look at the content of lee_position_controller.cpp in detail. The code and its comments are as follows (the function analysis below)
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;
}
The angular_acc_to_rotor_velocities_ matrix is the right pseudo-inverse of the coefficient matrix controller_parameters_.allocation_matrix_: A -1 =A T (A A T) -1
The reason for using the pseudo-inverse is that the coefficient matrix does not have an inverse
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();
}
To summarize, here is the operation to calculate the expected input u = [u1 u2 u3 u4], and then convert u to the motor speed:
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;
}
To calculate the expected acceleration, first calculate the expected lift F, and then divide it by the mass m. The summary is:
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
}
}
The summary is:
In general, lee_position_controllerthe controller needs to pass in the odometer message and the target trajectory message, and then use the se3 method to calculate the expected motor speed
6、roll_pitch_yawrate_thrust_controller_node
There is also a control node under the folder. This control node is also controlled by se3. The code is similar to lee_position_controller_node. I won’t explain it here. Let’s talk about the difference between the two control nodes. The track information of the lee_position_controller_node control node includes ~/UAV_rotors/src/rotors_simulator/rotors_control/src/nodes( roll_pitch_yawrate_thrust_controller_nodexyz yaw), and the track message of roll_pitch_yawrate_thrust_controller_node includes (thrust size, roll, pitch, yawrate), which is a control node developed for the drone remote control, because the command given by the remote control when controlling the drone is the thrust value , roll angle, pitch angle, yawrate, only the yaw angle is the angular velocity of the three attitude angles here, because when the remote control is released, the roll and pitch return to 0 degrees, and the yaw angle remains unchanged