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;
}
