引言
本篇博文意在整体梳理一下geometric_control无人机控制器的算法逻辑,顺便加深以下自己对该算法的理解...
整个无人机的定位和控制执行流程如下:
LIO ——> uav_vison_control ——> geometric_control ——> FCU
/odometry ——> /mavros/vision_pose/pose ——> /mavros/local_position/pose ——> /mavros/setpoint_raw/attitude
首先通过视觉里程计(VIO)或激光里程计(LIO)获取无人机定位信息,然后将通过定位信息转换节点,将里程计通过/mavros/vision_pose/pose话题输入给无人机飞控,与飞控IMU作EKF融合后,通过/mavros/local_position/pose话题输出基于飞控坐标系的定位信息,以此来作为无人机的位置控制基准。
该算法总体围绕几大飞行状态展开:
起飞点载入------WAITING_FOR_HOME_POSE
起飞------TAKING_OFF
任务执行------MISSION_EXCUTION
任务保持------MISSION_HOLD
开始降落------LANDING
降落完成------LANDED
enum FlightState {
WAITING_FOR_HOME_POSE,
TAKING_OFF,
MISSION_EXECUTION,
MISSION_HOLD,
LANDING,
LANDED
} node_state;
飞行状态详解
控制算法程序通过如下控制循环回调函数进行,通过switch开关来进行不同的无人机控制状态
void geometricCtrl::cmdloopCallback(const ros::TimerEvent &event) {
switch (node_state) {
case WAITING_FOR_HOME_POSE:
case TAKING_OFF:
case MISSION_EXECUTION:
case MISSION_HOLD:
case LANDING:
case LANDED:
}
}
1. WAITING_FOR_HOME_POSE
首先订阅无人机飞控/mavros/local_position/pose,获取飞控位置
mavposeSub_ = nh_.subscribe("mavros/local_position/pose", 1,
&geometricCtrl::mavposeCallback, this,
ros::TransportHints().tcpNoDelay());
void geometricCtrl::mavposeCallback(const geometry_msgs::PoseStamped &msg) {
if (!received_home_pose) {
received_home_pose = true;
home_pose_ = msg.pose;
ROS_INFO_STREAM("Home pose initialized to: " << home_pose_);
}
mavPos_ = toEigen(msg.pose.position);
mavAtt_(0) = msg.pose.orientation.w;
mavAtt_(1) = msg.pose.orientation.x;
mavAtt_(2) = msg.pose.orientation.y;
mavAtt_(3) = msg.pose.orientation.z;
}
执行case:WAITING_FOR_HOME_POSE
waitForPredicate()获取无人机初始位置,获取到无人机位置后,切换为TAKING_OFF起飞状态
case WAITING_FOR_HOME_POSE:
waitForPredicate(&received_home_pose, "Waiting for home pose...");
ROS_INFO("Got pose! Drone Ready to be armed.");
// node_state = MISSION_EXECUTION;
node_state = TAKING_OFF;
break;
void waitForPredicate(const T *pred, const std::string &msg,
double hz = 2.0) {
ros::Rate pause(hz);
ROS_INFO_STREAM(msg);
while (ros::ok() && !(*pred)) {
ros::spinOnce();
pause.sleep();
}
}
2. TAKING_OFF
执行case:TAKING_OFF
通过/mavros/setpoint_position/local话题,发布起飞目标点位置信息:initTargetPos_x_,initTargetPos_y_,initTargetPos_z_,初始位置由rosparam手动提供,初定为(0, 0, 1)点
nh_private_.param<double>("init_pos_x", initTargetPos_x_, 0.0);
nh_private_.param<double>("init_pos_y", initTargetPos_y_, 0.0);
nh_private_.param<double>("init_pos_z", initTargetPos_z_, 1.0);
case TAKING_OFF: {
geometry_msgs::PoseStamped takingoff_msg;
takingoff_msg.header.stamp = ros::Time::now();
takingoff_msg.pose.position.x = initTargetPos_x_;
takingoff_msg.pose.position.y = initTargetPos_y_;
takingoff_msg.pose.position.z = initTargetPos_z_;
target_pose_pub_.publish(takingoff_msg);
ros::spinOnce();
break;
}
target_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>(
"mavros/setpoint_position/local", 10);
3. MISSION_EXCUTION
当路径规划模块通过/planning/pos_cmd话题发布路径信息时,无人机进入任务执行状态,执行case:MISSION_EXCUTION
quadcmdSub_ =
nh_.subscribe("planning/pos_cmd", 1, &geometricCtrl::quadmsgCallback,
this, ros::TransportHints().tcpNoDelay());
从/planning/pos_cmd话题获取规划路径的目标点,目标速度,目标加速度和航向角
void geometricCtrl::quadmsgCallback(
const quadrotor_msgs::PositionCommand::ConstPtr &cmd) {
node_state = MISSION_EXECUTION;
reference_request_last_ = reference_request_now_;
targetPos_prev_ = targetPos_;
targetVel_prev_ = targetVel_;
reference_request_now_ = ros::Time::now();
cmdpos_time_last_ = reference_request_now_;
reference_request_dt_ =
(reference_request_now_ - reference_request_last_).toSec();
targetPos_ =
Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
targetVel_ =
Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
targetAcc_ = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y,
cmd->acceleration.z);
targetJerk_ = Eigen::Vector3d::Zero();
targetSnap_ = Eigen::Vector3d::Zero();
mavYaw_ = double(cmd->yaw);
// cmdBodyRate_[2] = cmd->yaw_dot;
}
执行case:MISSION_EXCUTION。总体分为几个模块和步骤:
1. 计算机身速度
2. 发布参考位姿
3. 发布控制指令(xyz欧拉角和油门大小)
4. 添加历史点
5. 发布历史点
case MISSION_EXECUTION:
{
std::cout << "mission execution " << dtttt << std::endl;
if (!feedthrough_enable_)
computeBodyRateCmd(cmdBodyRate_, targetPos_, targetVel_, targetAcc_);
pubReferencePose(targetPos_, q_des);
pubRateCommands(cmdBodyRate_);
appendPoseHistory();
pubPoseHistory();
ros::spinOnce();
// }
break;
}
1. 计算机身速度
ctrl_mode_ == ERROR_QUATERNION,进入非线性的无人机姿态控制器attcontroller()
void geometricCtrl::computeBodyRateCmd(Eigen::Vector4d &bodyrate_cmd,
const Eigen::Vector3d &target_pos,
const Eigen::Vector3d &target_vel,
const Eigen::Vector3d &target_acc) {
/// Compute BodyRate commands using differential flatness
/// Controller based on Faessler 2017
const Eigen::Vector3d a_ref = target_acc;
if (velocity_yaw_) {
mavYaw_ = getVelocityYaw(mavVel_);
}
const Eigen::Vector4d q_ref = acc2quaternion(a_ref - g_, mavYaw_);
const Eigen::Matrix3d R_ref = quat2RotMatrix(q_ref);
const Eigen::Vector3d pos_error = mavPos_ - target_pos;
const Eigen::Vector3d vel_error = mavVel_ - target_vel;
std::cout << "the position error is: " << pos_error(2) << std::endl;
Eigen::Vector3d a_fb =
Kpos_.asDiagonal() * pos_error +
Kvel_.asDiagonal() * vel_error; // feedforward term for trajectory error
if (a_fb.norm() > max_fb_acc_)
a_fb = (max_fb_acc_ / a_fb.norm()) *
a_fb; // Clip acceleration if reference is too large
const Eigen::Vector3d a_rd =
R_ref * D_.asDiagonal() * R_ref.transpose() * target_vel; // Rotor drag
const Eigen::Vector3d a_des = a_fb + a_ref - a_rd - g_;
q_des = acc2quaternion(a_des, mavYaw_);
if (ctrl_mode_ == ERROR_GEOMETRIC) {
bodyrate_cmd =
geometric_attcontroller(q_des, a_des, mavAtt_); // Calculate BodyRate
} else {
bodyrate_cmd = attcontroller(q_des, a_des, mavAtt_); // Calculate BodyRate
}
}
计算返回ratecmd。ratecmd(0),ratecmd(1),ratecmd(2)分别是x,y,z方向的欧拉角,ratecmd(3)为无人机油门大小
Eigen::Vector4d geometricCtrl::attcontroller(const Eigen::Vector4d &ref_att,
const Eigen::Vector3d &ref_acc,
Eigen::Vector4d &curr_att) {
// Geometric attitude controller
// Attitude error is defined as in Brescianini, Dario, Markus Hehn, and
// Raffaello D'Andrea. Nonlinear quadrocopter attitude control: Technical
// report. ETH Zurich, 2013.
Eigen::Vector4d ratecmd;
Eigen::Vector4d qe, q_inv, inverse;
Eigen::Matrix3d rotmat;
Eigen::Vector3d zb;
inverse << 1.0, -1.0, -1.0, -1.0;
q_inv = inverse.asDiagonal() * curr_att;
qe = quatMultiplication(q_inv, ref_att);
ratecmd(0) = (2.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(1);
ratecmd(1) = (2.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(2);
ratecmd(2) = (1.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(3);
rotmat = quat2RotMatrix(mavAtt_);
zb = rotmat.col(2);
ratecmd(3) =
std::max(0.0, std::min(1.0, norm_thrust_const_ * ref_acc.dot(zb) +
norm_thrust_offset_)); // Calculate thrust
return ratecmd;
}
2. 发布参考位置和姿态
referencePosePub_ =
nh_.advertise<geometry_msgs::PoseStamped>("reference/pose", 1);
void geometricCtrl::pubReferencePose(const Eigen::Vector3d &target_position,
const Eigen::Vector4d &target_attitude) {
geometry_msgs::PoseStamped msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = "map";
msg.pose.position.x = target_position(0);
msg.pose.position.y = target_position(1);
msg.pose.position.z = target_position(2);
msg.pose.orientation.w = target_attitude(0);
msg.pose.orientation.x = target_attitude(1);
msg.pose.orientation.y = target_attitude(2);
msg.pose.orientation.z = target_attitude(3);
referencePosePub_.publish(msg);
}
3. 发布控制指令
发布到/mavros/setpoint_raw/attitude话题,输入给无人机飞控(FCU),无人机获取信息,并做出相应相应。到此,无人机就完成了一次从获取路径到路径跟随执行的完整过程。
void geometricCtrl::pubRateCommands(const Eigen::Vector4d &cmd) {
mavros_msgs::AttitudeTarget msg;
double maxVelocity = 6.0;
geometry_msgs::TwistStamped velocity_msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = "map";
msg.body_rate.x = cmd(0);
msg.body_rate.y = cmd(1);
msg.body_rate.z = cmd(2);
msg.type_mask = 128; // Ignore orientation messages
msg.thrust = cmd(3);
// Publish velocity cmd (PI controller)
const Eigen::Vector3d pos_error = targetPos_ - mavPos_;
const Eigen::Vector3d vel_error = targetVel_ - mavVel_;
Eigen::Vector3d v_fb = 0.1 * Kpos_.asDiagonal() * pos_error + 0.1 * Kvel_.asDiagonal() * vel_error; // feedback term for trajectory error
angularVelPub_.publish(msg);
// velocity_msg.twist.linear.x = satfunc(velocity_msg.twist.linear.x, maxVelocity); //控制输出的最大线速度,当参考速度大于设置的最大速度,取设置速度
// velocity_msg.twist.linear.y = satfunc(velocity_msg.twist.linear.y, maxVelocity);
// velocity_msg.twist.linear.z = satfunc(velocity_msg.twist.linear.z, 0.7);
// target_velocity_pub_.publish(velocity_msg);
}
angularVelPub_ =
nh_.advertise<mavros_msgs::AttitudeTarget>("command/bodyrate_command", 1); //mavros/setpoint_raw/attitude