EGO-Planner的工作流程可以概括为：

• 初始化阶段：系统启动，初始化各个模块，等待触发信号
• 目标获取：接收目标点（手动、预设或路径跟踪）
• 轨迹规划：使用A搜索初始路径，B样条优化生成平滑轨迹
• 轨迹输出：将B样条轨迹转换为位置指令
• 轨迹执行：SO3控制器执行轨迹跟踪
• 状态监控：监控执行状态，判断是否需要重规划
• 循环执行：根据状态机逻辑循环执行上述过程

这个工作流程确保了四旋翼能够在复杂环境中安全、高效地进行自主导航。

展开代码
┌─────────────────────────────────────────────────────────────────────────────────┐
│                              EGO-Planner 工作流程                                │
└─────────────────────────────────────────────────────────────────────────────────┘

┌─────────────────┐
│   系统启动      │
│ ego_planner_node│
└─────────┬───────┘
          │
          ▼
┌─────────────────┐
│   初始化模块    │
│ EGOReplanFSM    │
│ - 读取参数      │
│ - 初始化状态机  │
│ - 设置定时器    │
└─────────┬───────┘
          │
          ▼
┌─────────────────┐
│   INIT状态      │
│ 等待里程计数据  │
│ 等待触发信号    │
└─────────┬───────┘
          │
          ▼
┌─────────────────┐
│  WAIT_TARGET    │
│ 等待目标点      │
└─────────┬───────┘
          │
          ▼
┌─────────────────┐
│   目标点输入    │
│ 手动/预设/路径  │
└─────────┬───────┘
          │
          ▼
┌─────────────────┐
│  GEN_NEW_TRAJ   │
│ 生成新轨迹      │
└─────────┬───────┘
          │
          ▼
┌─────────────────────────────────────────────────────────────────────────────────┐
│                              轨迹规划核心流程                                    │
├─────────────────────────────────────────────────────────────────────────────────┤
│                                                                                 │
│  ┌─────────────────┐    ┌─────────────────┐    ┌─────────────────┐              │
│  │   STEP 1: INIT  │───▶│  STEP 2: OPTIMIZE│───▶│ STEP 3: REFINE  │              │
│  │   初始化路径    │    │   B样条优化     │    │  轨迹可行性检查  │              │
│  │                 │    │                 │    │  时间重分配     │              │
│  │ - 多项式轨迹    │    │ - 多目标优化    │    │ - 速度约束检查  │              │
│  │ - A*路径搜索    │    │ - 平滑性代价    │    │ - 加速度约束    │              │
│  │ - 控制点生成    │    │ - 可行性代价    │    │ - 时间缩放      │              │
│  │                 │    │ - 距离代价      │    │                 │              │
│  └─────────────────┘    └─────────────────┘    └─────────────────┘              │
│                                                                                 │
└─────────────────────────────────────────────────────────────────────────────────┘
          │
          ▼
┌─────────────────┐
│   B样条输出     │
│ ego_planner::   │
│ Bspline消息     │
│ - 控制点        │
│ - 节点向量      │
│ - 轨迹ID        │
└─────────┬───────┘
          │
          ▼
┌─────────────────┐
│   轨迹服务器    │
│ traj_server     │
│ - 接收B样条     │
│ - 计算位置指令  │
│ - 100Hz发布     │
└─────────┬───────┘
          │
          ▼
┌─────────────────┐
│   位置指令      │
│ PositionCommand │
│ - 位置(x,y,z)   │
│ - 速度(vx,vy,vz)│
│ - 加速度(ax,ay,az)│
└─────────┬───────┘
          │
          ▼
┌─────────────────┐
│   SO3控制器     │
│ so3_control     │
│ - 力控制计算    │
│ - 姿态计算      │
│ - 推力分配      │
└─────────┬───────┘
          │
          ▼
┌─────────────────┐
│   四旋翼仿真器  │
│ quadrotor_sim   │
│ - 动力学仿真    │
│ - 状态更新      │
└─────────┬───────┘
          │
          ▼
┌─────────────────┐
│   状态反馈      │
│ 里程计/传感器   │
│ - 位置反馈      │
│ - 速度反馈      │
└─────────┬───────┘
          │
          ▼
┌─────────────────┐
│  EXEC_TRAJ状态  │
│ 轨迹执行监控    │
└─────────┬───────┘
          │
          ▼
┌─────────────────┐
│   重规划判断    │
│ needReplan()    │
│ - 环境变化检测  │
│ - 轨迹偏差检查  │
│ - 时间阈值检查  │
└─────────┬───────┘
          │
          ├─────────────────┐
          │                 │
          ▼                 ▼
┌─────────────────┐    ┌─────────────────┐
│   继续执行      │    │   触发重规划    │
│ 轨迹正常        │    │ 环境变化/超时   │
└─────────┬───────┘    └─────────┬───────┘
          │                      │
          │                      ▼
          │              ┌─────────────────┐
          │              │  REPLAN_TRAJ    │
          │              │ 重规划轨迹      │
          │              └─────────┬───────┘
          │                        │
          │                        ▼
          │              ┌─────────────────┐
          │              │  planFromCurrent│
          │              │ 从当前轨迹重规划│
          │              └─────────┬───────┘
          │                        │
          └────────────────────────┘
                                  │
                                  ▼
                       ┌─────────────────┐
                       │   任务完成      │
                       │   或循环继续    │
                       └─────────────────┘

关键接口和消息类型

轨迹数据：ego_planner::Bspline

• EGO-Planner的主要输出：B样条轨迹，过ROS消息ego_planner::Bspline发布：
• 包含B样条控制点和节点向量
• 由EGO-Planner发布到/planning/bspline话题

位置指令：quadrotor_msgs::PositionCommand

• 包含位置、速度、加速度、偏航角
• 由轨迹服务器发布到/planning/pos_cmd话题

SO3控制指令：quadrotor_msgs::SO3Command

• 包含期望的力和姿态四元数
• 由SO3控制器发布到so3_cmd话题

EGO-Planner与外环控制的结合方式

只需要关注EGO-Planner的轨迹生成和SO3控制器的设计，底层的电机控制和动力学仿真已经由仿真环境处理好了。

1. 数据流架构

展开代码
EGO-Planner → 轨迹服务器 → SO3控制器 → 四旋翼仿真器

2. 具体结合过程

第一步：EGO-Planner生成轨迹

cpp
展开代码
// ego_replan_fsm.cpp 中的状态机
case GEN_NEW_TRAJ:
{
    bool success = callReboundReplan(true, flag_random_poly_init);
    if (success) {
        changeFSMExecState(EXEC_TRAJ, "FSM");
    }
}

第二步：轨迹服务器接收并处理轨迹

cpp
展开代码
// traj_server.cpp
void bsplineCallback(ego_planner::BsplineConstPtr msg)
{
    // 解析B样条轨迹
    UniformBspline pos_traj(pos_pts, msg->order, 0.1);
    
    // 计算位置、速度、加速度
    pos = traj_[0].evaluateDeBoorT(t_cur);
    vel = traj_[1].evaluateDeBoorT(t_cur);
    acc = traj_[2].evaluateDeBoorT(t_cur);
    
    // 发布位置指令
    pos_cmd_pub.publish(cmd);
}

第三步：SO3控制器接收位置指令

cpp
展开代码
// so3_control_nodelet.cpp
void position_cmd_callback(const quadrotor_msgs::PositionCommand::ConstPtr& cmd)
{
    des_pos_ = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
    des_vel_ = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
    des_acc_ = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z);
    
    // 计算控制输出
    publishSO3Command();
}

第四步：SO3控制器计算力和姿态

cpp
展开代码
void publishSO3Command(void)
{
    // 计算所需的力和期望姿态
    controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_,
                               des_yaw_dot_, kx_, kv_);
    
    const Eigen::Vector3d& force = controller_.getComputedForce();
    const Eigen::Quaterniond& orientation = controller_.getComputedOrientation();
    
    // 发布SO3控制指令
    so3_command_pub_.publish(so3_command);
}

第五步：四旋翼仿真器执行控制

cpp
展开代码
// quadrotor_simulator_so3.cpp
static Control getControl(const QuadrotorSimulator::Quadrotor& quad, const Command& cmd)
{
    // 根据期望的力和姿态计算电机转速
    float w_sq[4];
    w_sq[0] = force / (4 * kf) - M2 / (2 * d * kf) + M3 / (4 * km);
    w_sq[1] = force / (4 * kf) + M2 / (2 * d * kf) + M3 / (4 * km);
    w_sq[2] = force / (4 * kf) + M1 / (2 * d * kf) - M3 / (4 * km);
    w_sq[3] = force / (4 * kf) - M1 / (2 * d * kf) - M3 / (4 * km);
    
    // 返回电机转速控制指令
    return control;
}