本文将详细介绍如何使用ego-planner向无人机发送控制指令，实现自动控制的过程。我们将探讨ego-planner的运行方式以及控制指令的传输过程，涉及到ROS节点、MAVROS协议以及无人机的控制器等方面。

在实现自动控制时，控制指令的传输是至关重要的一环。本文将详细介绍ego-planner如何向无人机发送控制指令，从而实现自动控制的过程。

无人机控制流程：

1. 运行run_ctrl.launch实现自动起飞和悬停。
2. 可通过遥控器或机载电脑进行手动或自动控制。

本文主要讲解自动控制中的控制指令传输过程。
控制指令传输过程：
在ego-planner中，控制指令的传输是通过px4ctrl_node.cpp实现的，该节点利用MAVROS将控制指令发送给无人机。（realflight_modules/px4ctrl/src/px4ctrl_node.cpp）

ros::Subscriber cmd_sub = nh.subscribe<quadrotor_msgs::PositionCommand>("cmd",100,boost::bind(&Command_Data_t::feed, &fsm.cmd_data, _1),ros::VoidConstPtr(),ros::TransportHints().tcpNoDelay());

fsm.ctrl_FCU_pub = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 10);

在控制有限状态机中，当状态为CMD_CTRL时，通过get_cmd_des()函数获取控制指令的期望状态。

case CMD_CTRL:
{if (!rc_data.is_hover_mode || !odom_is_received(now_time)){state = MANUAL_CTRL;toggle_offboard_mode(false);ROS_WARN("[px4ctrl] From CMD_CTRL(L3) to MANUAL_CTRL(L1)!");}else if (!rc_data.is_command_mode || !cmd_is_received(now_time)){state = AUTO_HOVER;set_hov_with_odom();des = get_hover_des();ROS_INFO("[px4ctrl] From CMD_CTRL(L3) to AUTO_HOVER(L2)!");}else{des = get_cmd_des();}...break;
}

控制指令获取过程：
在get_cmd_des()函数中，获取控制指令的期望状态des，包括位置、速度、加速度、加加速度、偏航角、偏航角速度。

Desired_State_t PX4CtrlFSM::get_cmd_des()
{Desired_State_t des;des.p = cmd_data.p;des.v = cmd_data.v;des.a = cmd_data.a;des.j = cmd_data.j;des.yaw = cmd_data.yaw;des.yaw_rate = cmd_data.yaw_rate;return des;
}

设定期望值des后，控制器负责根据获取的控制指令更新无人机的状态，其中包括推力模型的估计、控制指令的计算以及控制命令的发布。

// STEP2: estimate thrust modelif (state == AUTO_HOVER || state == CMD_CTRL){// controller.estimateThrustModel(imu_data.a, bat_data.volt, param);controller.estimateThrustModel(imu_data.a,param);}// STEP3: solve and update new control commandsif (rotor_low_speed_during_land) // used at the start of auto takeoff{motors_idling(imu_data, u);}else{debug_msg = controller.calculateControl(des, odom_data, imu_data, u);debug_msg.header.stamp = now_time;debug_pub.publish(debug_msg);}// STEP4: publish control commands to mavrosif (param.use_bodyrate_ctrl){publish_bodyrate_ctrl(u, now_time);}else{publish_attitude_ctrl(u, now_time);}

cmd_data 通过px4ctrl中subscriber接收得到（见上面subscriber中定义）。

可以看到STEP4有两种情况：bodyrate_ctrl和attitude_ctrl。看一下函数定义：

void PX4CtrlFSM::publish_bodyrate_ctrl(const Controller_Output_t &u, const ros::Time &stamp)
{mavros_msgs::AttitudeTarget msg;msg.header.stamp = stamp;msg.header.frame_id = std::string("FCU");msg.type_mask = mavros_msgs::AttitudeTarget::IGNORE_ATTITUDE;msg.body_rate.x = u.bodyrates.x();msg.body_rate.y = u.bodyrates.y();msg.body_rate.z = u.bodyrates.z();msg.thrust = u.thrust;ctrl_FCU_pub.publish(msg);
}

void PX4CtrlFSM::publish_attitude_ctrl(const Controller_Output_t &u, const ros::Time &stamp)
{mavros_msgs::AttitudeTarget msg;msg.header.stamp = stamp;msg.header.frame_id = std::string("FCU");msg.type_mask = mavros_msgs::AttitudeTarget::IGNORE_ROLL_RATE |mavros_msgs::AttitudeTarget::IGNORE_PITCH_RATE |mavros_msgs::AttitudeTarget::IGNORE_YAW_RATE;msg.orientation.x = u.q.x();msg.orientation.y = u.q.y();msg.orientation.z = u.q.z();msg.orientation.w = u.q.w();msg.thrust = u.thrust;ctrl_FCU_pub.publish(msg);
}

这两个函数都是用来发布飞行器的控制指令的，但是它们控制的方式略有不同。
publish_bodyrate_ctrl 函数发布的是基于体轴角速度（Body Rate）的控制指令。这种控制方式直接指定飞行器绕体轴的角速度，也就是说，控制器会直接告诉飞行器应该以多快的速度绕着自身的 X、Y、Z 轴旋转。

publish_attitude_ctrl 函数发布的是基于姿态角（Attitude）的控制指令。这种控制方式指定的是飞行器的期望姿态，也就是说，控制器会告诉飞行器应该朝向哪个方向。该函数忽略了角速度，只指定了期望的姿态。飞行器会根据姿态控制算法自行调整引擎输出以达到期望的姿态。这种控制方式适用于需要控制飞行器朝向的情况，比如指定飞行器俯仰、横滚和偏航的角度。

查找参数设置，可以在realflight_modules/px4ctrl/config/strl_param_fpv.yaml中看到

use_bodyrate_ctrl: false

这说明在该控制器中实际采用的是姿态控制。

在realflight_modules/px4ctrl/launch/run_ctrl.launch中，进行了话题重映射

remap from="~cmd" to="/position_cmd" />

这说明控制器实际接收话题名称为/position_cmd 接下来我们寻找ego-planner中发布该指令的位置。

在/src/planner/plan_manage/src/traj_server.cpp中可以看到控制指令（位置控制）发布者的定义

pos_cmd_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/position_cmd", 50);

顺藤摸瓜找到其发布消息的函数

void cmdCallback(const ros::TimerEvent &e)
{/* no publishing before receive traj_ */if (!receive_traj_)return;ros::Time time_now = ros::Time::now();double t_cur = (time_now - start_time_).toSec();Eigen::Vector3d pos(Eigen::Vector3d::Zero()), vel(Eigen::Vector3d::Zero()), acc(Eigen::Vector3d::Zero()), pos_f;std::pair<double, double> yaw_yawdot(0, 0);static ros::Time time_last = ros::Time::now();if (t_cur < traj_duration_ && t_cur >= 0.0){pos = traj_[0].evaluateDeBoorT(t_cur);vel = traj_[1].evaluateDeBoorT(t_cur);acc = traj_[2].evaluateDeBoorT(t_cur);/*** calculate yaw ***/yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last);/*** calculate yaw ***/double tf = min(traj_duration_, t_cur + 2.0);pos_f = traj_[0].evaluateDeBoorT(tf);}else if (t_cur >= traj_duration_){/* hover when finish traj_ */pos = traj_[0].evaluateDeBoorT(traj_duration_);vel.setZero();acc.setZero();yaw_yawdot.first = last_yaw_;yaw_yawdot.second = 0;pos_f = pos;return;}else{cout << "[Traj server]: invalid time." << endl;}time_last = time_now;cmd.header.stamp = time_now;cmd.header.frame_id = "world";cmd.trajectory_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY;cmd.trajectory_id = traj_id_;cmd.position.x = pos(0);cmd.position.y = pos(1);cmd.position.z = pos(2);cmd.velocity.x = vel(0);cmd.velocity.y = vel(1);cmd.velocity.z = vel(2);cmd.acceleration.x = acc(0);cmd.acceleration.y = acc(1);cmd.acceleration.z = acc(2);cmd.yaw = yaw_yawdot.first;cmd.yaw_dot = yaw_yawdot.second;last_yaw_ = cmd.yaw;pos_cmd_pub.publish(cmd);
}

该函数为控制频率100Hz执行（traj_server.cpp, line 243）

ros::Timer cmd_timer = nh.createTimer(ros::Duration(0.01), cmdCallback);

查看该函数内容，可以发现cmd所需的位置、速度、加速度均通过轨迹直接计算，计算函数为evaluateDeBoorT。偏航角yaw、其变化率yawdot通过另一个函数calculate_yaw计算。

calculate_yaw函数定义（traj_server.cpp, line 71）为：

std::pair<double, double> calculate_yaw(double t_cur, Eigen::Vector3d &pos, ros::Time &time_now, ros::Time &time_last)
{constexpr double PI = 3.1415926;constexpr double YAW_DOT_MAX_PER_SEC = PI;// constexpr double YAW_DOT_DOT_MAX_PER_SEC = PI;std::pair<double, double> yaw_yawdot(0, 0);double yaw = 0;double yawdot = 0;Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? traj_[0].evaluateDeBoorT(t_cur + time_forward_) - pos : traj_[0].evaluateDeBoorT(traj_duration_) - pos;double yaw_temp = dir.norm() > 0.1 ? atan2(dir(1), dir(0)) : last_yaw_;double max_yaw_change = YAW_DOT_MAX_PER_SEC * (time_now - time_last).toSec();if (yaw_temp - last_yaw_ > PI){if (yaw_temp - last_yaw_ - 2 * PI < -max_yaw_change){yaw = last_yaw_ - max_yaw_change;if (yaw < -PI)yaw += 2 * PI;yawdot = -YAW_DOT_MAX_PER_SEC;}else{yaw = yaw_temp;if (yaw - last_yaw_ > PI)yawdot = -YAW_DOT_MAX_PER_SEC;elseyawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();}}else if (yaw_temp - last_yaw_ < -PI){if (yaw_temp - last_yaw_ + 2 * PI > max_yaw_change){yaw = last_yaw_ + max_yaw_change;if (yaw > PI)yaw -= 2 * PI;yawdot = YAW_DOT_MAX_PER_SEC;}else{yaw = yaw_temp;if (yaw - last_yaw_ < -PI)yawdot = YAW_DOT_MAX_PER_SEC;elseyawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();}}else{if (yaw_temp - last_yaw_ < -max_yaw_change){yaw = last_yaw_ - max_yaw_change;if (yaw < -PI)yaw += 2 * PI;yawdot = -YAW_DOT_MAX_PER_SEC;}else if (yaw_temp - last_yaw_ > max_yaw_change){yaw = last_yaw_ + max_yaw_change;if (yaw > PI)yaw -= 2 * PI;yawdot = YAW_DOT_MAX_PER_SEC;}else{yaw = yaw_temp;if (yaw - last_yaw_ > PI)yawdot = -YAW_DOT_MAX_PER_SEC;else if (yaw - last_yaw_ < -PI)yawdot = YAW_DOT_MAX_PER_SEC;elseyawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();}}if (fabs(yaw - last_yaw_) <= max_yaw_change)yaw = 0.5 * last_yaw_ + 0.5 * yaw; // nieve LPFyawdot = 0.5 * last_yaw_dot_ + 0.5 * yawdot;last_yaw_ = yaw;last_yaw_dot_ = yawdot;yaw_yawdot.first = yaw;yaw_yawdot.second = yawdot;return yaw_yawdot;
}

该段代码是一个用于计算航向角（yaw）及其变化率（yaw rate, 即yaw的一阶导数）的函数。函数接收当前时间t_cur、当前位置pos、当前时间time_now及上一次计算时间time_last作为输入，返回一对双精度浮点数，分别代表当前的航向角和航向角变化率。

函数逻辑解析如下：

1. 初始化常量与变量:

YAW_DOT_MAX_PER_SEC：航向角变化率的最大值，限制航向角的变化速度。
yaw_yawdot：存储计算结果的pair，初始为(0, 0)。
yaw和yawdot：分别用于存储计算出的航向角和航向角变化率。

2. 计算目标方向:

使用De Boor算法（用于计算B样条形式的样条曲线）计算给定时间点的目标位置，然后计算从当前位置pos到目标位置的方向向量dir。
$$\begin{gathered} dir=\text{DeBoor}(t\_cur+t\_forward)-pos,\text{ if }t\_cur+t\_forward\le t\_duration \\ dir=\text{DeBoor}(t\_duration)-pos,\text{ otherwise } \end{gathered}$$

3. 计算临时航向角:

yaw_temp：基于dir向量，使用atan2函数计算得到的临时航向角，当dir的模长大于0.1时有效，否则使用上一次的航向角last_yaw_。

4. 计算航向角变化限制:

max_yaw_change：根据YAW_DOT_MAX_PER_SEC和时间差(time_now - time_last)计算的最大航向角变化量。
$$\Delta yawmax=YAW\_DOT\_MAX\_PER\_SEC\times \Delta t$$

5. 调整航向角:

通过比较yaw_temp与last_yaw_的差值，考虑边界情况（如航向角跨越±π），并限制航向角的变化量不超过max_yaw_change，从而计算出新的yaw和yawdot。

6. 平滑处理:

对计算出的yaw和yawdot进行简单的低通滤波处理，以平滑航向角及其变化率。
$$\begin{gathered} yaw=0.5\times last\_yaw\_+0.5\times yaw \\ yawdot=0.5\times last\_yaw\_dot\_+0.5\times yawdot \\ yawdot=0.5\times last\_yaw\_dot\_+0.5\times yawdot \end{gathered}$$

7. 更新并返回结果:

更新last_yaw_和last_yaw_dot_为当前计算结果。将计算结果存入yaw_yawdot并返回。