【rotors】多旋翼无人机仿真(三)——SE3控制
在上一节中,我们分析了hovering_example节点程序,本节中我们来看一下最重要的控制节点lee_position_controller_node。
【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=AT (AAT)-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角是保持不变
![Logo](https://devpress.csdnimg.cn/79de2bf0b7994defa4242ef90d5513fa.jpg)
开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!
更多推荐
所有评论(0)