前言

实验目标

在之前的Astar规划baseline的基础上进行改进，让无人机能够选择已知地图外的点作为目标点，并能够边飞边规划，而非规划一次就直接飞到目标点。

实验配置&项目结构

参考上一篇baseline

改进分析

可以分为以下三部分
第一阶段：只做边飞边规划，目标仍要求在图内

• 新增 NAVIGATE
• 周期性重规划
• 路径失效检测
• 地图更新触发重规划
  这样能先验证“滚动规划框架”是稳定的。

第二阶段：支持地图外全局目标

• 拆分 global_goal_ / local_goal_
• 增加 worldInMap() 和 computeLocalGoal()
• 加入边界裁剪逻辑

第三阶段：优化导航品质

• 前视点跟踪替代逐格点跟踪
• 局部窗口规划或路径平滑
• frontier 或探索式子目标选择

分点改进

滚动路径规划

当前的核心逻辑在于fsm中的wait_goal调用一次plan便进入execute，于是需要拆分该逻辑。首先将execute模块更名为navigate，当wait_goal得到目标和有地图后便切换到navigate，而后续一直在该模式下进行执行路径与定时重规划。因此主要是对mission_fsm部分代码进行修改

fsm关键逻辑

加入一个need_replan_变量，当为真时重新进行路径规划。而当地图更新、周期超时、路径失效和设置新目标点时会触发重新规划，重新规划后替换路径。

void MissionFSM::execNavigate()
{
    auto cur = mavros_utils_.getCurrentPose().pose.position;
    auto goal = global_goal_.pose.position;
    double goal_dist = std::hypot(cur.x - goal.x, cur.y - goal.y);

    // 以目标距离而不是“路径是否走完”作为任务完成条件
    if (goal_dist < waypoint_threshold_)
    {
		...
        need_replan_ = false;
        current_state_ = State::WAIT_GOAL;
        return;
    }

    // 地图一旦更新，就触发重规划以利用最新环境信息
    ros::Time map_time = planner_.getLastMapUpdateTime();
    if (map_time > last_map_time_)
    {
        last_map_time_ = map_time;
        need_replan_ = true;
    }

    // 没有可执行路径时，必须规划
    if (current_path_.poses.empty())
        need_replan_ = true;

    // 周期性重规划，避免一直沿用旧路径
    if ((ros::Time::now() - last_plan_time_).toSec() > replan_period_)
        need_replan_ = true;

    // 若当前路径被新地图判定为无效，则强制重规划
    bool path_still_valid = !current_path_.poses.empty() &&
                            planner_.isPathStillValid(current_path_, path_idx_);
    if (!current_path_.poses.empty() && !path_still_valid)
        need_replan_ = true;

    if (need_replan_)
    {
        // 以当前位置为起点，对地图内局部目标滚动重规划
		...
    }
}

改进结果

启动rviz后添加目标点，可以发现虽然目标点还是只能在已知地图内，但是路径在无人机飞行过程中会不断变化，而不是之前的固定路径。

存在问题

现在navigation模式承载了太多的作用，又要规划路径，又要执行路径。观察ego_planner的代码，发现也还是区分了规划和执行模式。其次，虽然路径会实时规划了，但是总体的路径变得没有之前平滑。

全局目标支持

在现有的代码基础上，为了支持全局目标，需要不再将给出的目标点当作A* 的规划终点，而是拆分为global_goal和local_goal，分别表示全局目标点和当前地图内的目标点。

关键逻辑

具体到代码上，每次触发重新规划时，不再直接使用global_goal，而是先判断其是否在当前地图内，如果在则local_goal=global_goal，否则去计算一个local_goal。而计算方法也很简单，即当前位置和目标点的连线，与地图边界的交点。

if (need_replan_)
{
    bool goal_in_map = planner_.worldInMap(goal.x, goal.y);
    if (goal_in_map)
    {
        local_goal_ = goal;
        ROS_INFO("Replanning directly to global goal");
    }
    else
    {
        local_goal_ = planner_.computeLocalGoal(cur, goal, local_goal_margin_);
        ROS_INFO("Global goal is outside map, replanning toward local goal (%.2f, %.2f)",
                 local_goal_.x, local_goal_.y);
    }

改进结果

现在可以选择已知地图以外的点作为目标点了，rviz中可以看到路线始终在已知地图的范围内，但是随着无人机飞行在不断更新。
但是如果无人机一旦发现目标点在障碍物内，则会原地停下（判断路线无效）

优化导航

前视路径点

在当前的路径执行中，无人机是对路径中的点逐个遍历，但是当路径点很密集时，无人机会频繁地修正方向，导致飞行抖动。而前视路径点的意义就是不再选择最近的一个路径点作为目标，而是选择前方具有一定距离的点。

路径平滑

将抖动的线段整合为直线

其他细节

添加无人机模型

要在rviz中显示无人机模型，需要将无人机模型的.dae文件作为mesh marker发布，然后在rviz的marker下订阅即可。

实验结果

相较于之前的基础实验，经过上述更改后，无人机实现了边飞边规划，并且能够选择已知地图以外的目标点，且飞行路线更加平滑。

后续改进

明显可以看出，用A* 得到的路径还是非常抖动，最好是切换成用多项式表达的曲线。其次，octomap包默认是维持全局地图，如果需要局部地图，要么更换其他包，要么自己写。再来，nav模式承载了太多功能，最好分离出来。