无人机自主探索FUEL：代码阅读3–执行循环顺序与部分释义

一、循环

执行
fast_exploration_manager.cpp
中的
FastExplorationManager::planExplore
，进行探索并选择视点；

此时终端打印max_id，min_id，代价cost mat以及旅行商算法参数 TSP

— 相当于对应原文的第一部分，建立FIS，找到边界簇并不断更新信息，找到视点
执行
fast_exploration_manager.cpp
中的
FastExplorationManager::planMotion
：

其中主要调用了两个函数
kinodynamicReplan
,
planExploreTraj
和
planYawExplore
，位于
planner_manager.cpp

其中当前视点与下一个视点距离

不超过

上下限时，调用
kinodynamicReplan
得到规划路径；

超过

上下限时，调用
planExploreTraj
得到规划路径。

— 对应原文第二部分，根据视点进行规划得到最终轨迹

    int FastExplorationManager::planMotion(
            const Vector3d &cur_pos, const Vector3d &cur_vel, const Vector3d &cur_acc, const Eigen::Vector3d &cur_yaw,
            const Eigen::Vector3d &next_pos, const double next_yaw) {
        
        std::cout<< "&&&&&&&&&&&&&&&&&& planMotion &&&&&&&&&&&&&&&&&&&&"<<std::endl;

        double diff = fabs(next_yaw - cur_yaw[0]);                        //计算预期yaw角差距的绝对值
        double time_lb = min(diff, 2 * M_PI - diff) / ViewNode::yd_;          // 

        planner_manager_->astar_path_finder_->reset();
        if (planner_manager_->astar_path_finder_->search(cur_pos, next_pos) != Astar::REACH_END) {       //检查到下一个视点是否有障碍物
            ROS_ERROR("No path to next viewpoint");
            return FAIL;
        }
        auto path_to_next_goal = planner_manager_->astar_path_finder_->getPath();
        shortenPath(path_to_next_goal);

        const double radius_far = 5.0;            // 如果视点距离大于far，则只取 far的距离
        const double radius_close = 1.5;          //原1.5  如果视点距离小于close则不使用kino，直接使用两个视点的信息进行优化得到轨迹
        const double full_path_len = Astar::pathLength(path_to_next_goal);
        if (full_path_len < radius_close) {
            std::cout<< "------------ length of path < radius_close --------------"<<std::endl;
            // Next viewpoint is very close, no need to search kinodynamic path, just use waypoints-based optimization
            planner_manager_->planExploreTraj(path_to_next_goal, cur_vel, cur_acc, time_lb);
        } 
        else if (full_path_len > radius_far) {
            std::cout<< "------------ length of path > radius_far --------------"<<std::endl;
            // Next viewpoint is far away, select intermediate goal on geometric path (this also deal with dead end)
            double len2 = 0.0;
            vector<Eigen::Vector3d> truncated_path = {path_to_next_goal.front()};
            for (size_t i = 1; i < path_to_next_goal.size() && len2 < radius_far; ++i) {
                auto cur_pt = path_to_next_goal[i];
                len2 += (cur_pt - truncated_path.back()).norm();
                truncated_path.push_back(cur_pt);
            }
            planner_manager_->planExploreTraj(truncated_path, cur_vel, cur_acc, time_lb);     //在radius_far范围内规划轨迹
        } else {
            std::cout<< "------------ Search kino path to exactly next viewpoint --------------"<<std::endl;
            // Search kino path to exactly next viewpoint and optimize
            // cout << "\n\n\n\n\n\n\n\n0000000000000000000000000\n0000000000000000000000000\n\n\n\n\n\n";
            if (!planner_manager_->kinodynamicReplan(cur_pos, cur_vel, cur_acc, next_pos, Vector3d(0, 0, 0), time_lb))
                return FAIL;
        }

        // 以上不管使用planExploreTraj还是kinodynamicReplan，都仅使用p,v,a进行规划，没有规划yaw角

        if (planner_manager_->local_data_->pos_traj_.getTimeSum() < time_lb - 0.1)
            ROS_ERROR("Lower bound not satified!");

        LocalTrajDataPtr local_traj_data = planner_manager_->local_data_;
        local_traj_data->duration_ = local_traj_data->pos_traj_.getTimeSum();
        //单独对yaw角进行了规划
        planner_manager_->planYawExplore(cur_yaw, next_yaw, local_traj_data->pos_traj_,
                                         local_traj_data->duration_, ep_->relax_time_);

        local_traj_data->culcDerivatives();
        local_traj_data->start_time_ = ros::Time::now();
        local_traj_data->traj_id_ += 1;

        return SUCCEED;
    }

即三种情形下的规划分别为：

1）
if (full_path_len < radius_close)
：

planner_manager_->planExploreTraj(path_to_next_goal, cur_vel, cur_acc, time_lb);

2）
else if (full_path_len > radius_far)
：

planner_manager_->planExploreTraj(truncated_path, cur_vel, cur_acc, time_lb);

3）else:

if (!planner_manager_->kinodynamicReplan(cur_pos, cur_vel, cur_acc, next_pos, Vector3d(0, 0, 0), time_lb))
                return FAIL;

终端输出结果：

（自己加了一些其他输出打印）

&&&&&&&&&&&&&&&&&& planExplore 进行探索，选择视点 &&&&&&&&&&&&&&&&&&&&
[ WARN] [1690162858.128162765]: min_id 0.000000 0.000000 0.000000: 
[ WARN] [1690162858.128179720]: max_id 0.000000 0.000000 0.000000: 

[ WARN] [1690162858.170150992]: Cost mat: 0.035899, TSP: 0.001841
[Dijkstra Search] Node: 38, edge: 314
&&&&&&&&&&&&&&&&&& planMotion &&&&&&&&&&&&&&&&&&&&
=================  开始执行kino,获得kino起点终点信息==================
-- 在kino中调用A* search --
--------------KinodynamicAstar begin to search ----------------------
vel:-0.0602589, acc:-0.891468
near end
[non uniform B-spline] A size	120
============== B样条规划 yaw角  ===================
[planner manager] B-spline时间间隔: dt_yaw: 0.066089, start yaw:  -1.07579 0.0465283 0.0235393, end_yaw: -1.09138
[FSM]: from PLAN_TRAJ to EXEC_TRAJ
[FSM]: from EXEC_TRAJ to PLAN_TRAJ
[ WARN] [1690162858.479040533]: Replan: traj fully executed =======================================
&&&&&&&&&&&&&&&&&& planExplore 进行探索，选择视点 &&&&&&&&&&&&&&&&&&&&
[ WARN] [1690162858.489120813]: min_id 0.000000 0.000000 0.000000: 
[ WARN] [1690162858.489142242]: max_id 0.000000 0.000000 0.000000: 
[ WARN] [1690162858.492342561]: Frontier t: 0.003229

[ WARN] [1690162858.531257640]: Cost mat: 0.034793, TSP: 0.001260
[Dijkstra Search] Node: 36, edge: 276
&&&&&&&&&&&&&&&&&& planMotion &&&&&&&&&&&&&&&&&&&&
==============   B样条规划轨迹 (x,y,z)    ===============
[planner manager]  duration: 5.81806, seg_num: 8, dt: 0.727257
[non uniform B-spline] A size	143
============== B样条规划 yaw角  ===================
[planner manager] B-spline时间间隔: dt_yaw: 0.0753123, start yaw:  -1.06349 0.0148696 -0.117909, end_yaw: -1.03812
[FSM]: from PLAN_TRAJ to EXEC_TRAJ
[ INFO] [1690162858.906120884]: [FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to PLAN_TRAJ
[ WARN] [1690162858.949439946]: Replan: traj fully executed =======================================

二、规划部分

1.
`FastPlannerManager::kinodynamicReplan`
（当前视点与下一个视点距离不超限）

    bool FastPlannerManager::kinodynamicReplan(const Eigen::Vector3d &start_pt,
                                               const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
                                               const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel,
                                               const double &time_lb) {
                                            
        std::cout<< "================= next viewpoint不超限,使用kino开始规划 =================="<<endl;
        std::cout << "[Kino replan]: start: start_pt = " << start_pt.transpose() << ", start_vel = " << start_vel.transpose()
                  << ", start_acc = " << start_acc.transpose() << endl;
        std::cout << "[Kino replan]:  goal: end_pt = " << end_pt.transpose() << ", end_vel = "
                  << end_vel.transpose() << endl;

        if ((start_pt - end_pt).norm() < 1e-2) {
            cout << "起点终点距离过近, Close goal" << endl;
            return false;
        }

        /******************************
         * Kinodynamic path searching *
         ******************************/
        vector<PathNodePtr> path;
        double shot_time;
        Matrix34 coef_shot;
        bool is_shot_succ;
        std::cout << "-- 在kino中调用A* search 验证--" << std::endl;
        //最多使用search规划两次
        int status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel,
                                               true, path, is_shot_succ, coef_shot, shot_time);
        if (status == KinodynamicAstar::NO_PATH) {
            ROS_ERROR("[Kino replan]: search the first time fail");
            status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel,
                                               false, path, is_shot_succ, coef_shot, shot_time);    // 由true改为false
            if (status == KinodynamicAstar::NO_PATH) {
                cout << "[Kino replan]: Can't find path. " << endl;
                return false;
            }
        }

其中函数search主要用于寻找路径，输入起点的p,v,a,终点p,v，布尔值init_search，路点path，布尔值is_shot_succ，矩阵coef_shot，时间shot_time；当第一次找不到轨迹时布尔值布尔值init_search置为false，会再次调用：

int status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel,
                                               true, path, is_shot_succ, coef_shot, shot_time);

ROS_ERROR("[Kino replan]: search the first time fail");
            status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel,
                                               false, path, is_shot_succ, coef_shot, shot_time);    // 由true改为false

对应函数位于
kinodynamic_astar.cpp
：

int KinodynamicAstar::search(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_v, const Eigen::Vector3d &start_a,
                                 const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_v,
                                 const bool init_search, vector<PathNodePtr> &path,
                                 bool &is_shot_succ, Matrix34 &coef_shot, double &shot_time)

布尔值init_search置为true或者false的主要影响：

当为true时，
tau
的计算方法不一样

        if (init_search) {
            inputs.push_back(start_a);
            for (double tau = time_res_init * init_max_tau_; tau <= init_max_tau_ + 1e-3; tau += time_res_init * init_max_tau_)
                durations.push_back(tau);
        } 
        
        else {
            for (double ax = -max_acc_; ax <= max_acc_ + 1e-3; ax += max_acc_ * res)
                for (double ay = -max_acc_; ay <= max_acc_ + 1e-3; ay += max_acc_ * res)
                    for (double az = -max_acc_; az <= max_acc_ + 1e-3; az += max_acc_ * res) {
                        inputs.emplace_back(ax, ay, az);
                    }
            for (double tau = time_res * max_tau_; tau <= max_tau_; tau += time_res * max_tau_)
                durations.push_back(tau);
        }

添加两行输出辅助信息观察后：

if (init_search) {
            inputs.push_back(start_a);
            for (double tau = time_res_init * init_max_tau_; tau <= init_max_tau_ + 1e-3; tau += time_res_init * init_max_tau_){
                //-------------------   add     ---------------------
                std::cout<<"[search 1]: tau = "<< tau << std::endl;
                durations.push_back(tau);
            }
                
        } 
        else {
            for (double ax = -max_acc_; ax <= max_acc_ + 1e-3; ax += max_acc_ * res)
                for (double ay = -max_acc_; ay <= max_acc_ + 1e-3; ay += max_acc_ * res)
                    for (double az = -max_acc_; az <= max_acc_ + 1e-3; az += max_acc_ * res) {
                        inputs.emplace_back(ax, ay, az);
                    }
            for (double tau = time_res * max_tau_; tau <= max_tau_; tau += time_res * max_tau_){
                //-------------------   add     ---------------------
                std::cout<<"[search 2]: tau = "<< tau << std::endl;
                durations.push_back(tau);
            }
                
        }

终端信息：

================= next viewpoint不超限,使用kino开始规划 ==================
[Kino replan]: start: start_pt =  -1.47004 -0.750328   1.03817, start_vel = -0.213102 -0.490083 -0.168459, start_acc =  0.347463 -0.191323  0.178028
[Kino replan]:  goal: end_pt = -2.30097  2.81245  1.29608, end_vel = 0 0 0
-- 在kino中调用A* search 验证--
--------------KinodynamicAstar begin to search ----------------------
[search 1]: tau = 0.05
[search 1]: tau = 0.1
[search 1]: tau = 0.15
[search 1]: tau = 0.2
[search 1]: tau = 0.25
[search 1]: tau = 0.3
[search 1]: tau = 0.35
[search 1]: tau = 0.4
[search 1]: tau = 0.45
[search 1]: tau = 0.5
[search 1]: tau = 0.55
[search 1]: tau = 0.6
[search 1]: tau = 0.65
[search 1]: tau = 0.7
[search 1]: tau = 0.75
[search 1]: tau = 0.8
[search 1]: tau = 0.85
[search 1]: tau = 0.9
[search 1]: tau = 0.95
[search 1]: tau = 1
open set empty, no path!
use node num: 351
iter num: 28

最后对路径进行B样条优化：

     /*********************************
         * Parameterize path to B-spline *
         *********************************/
        double ts = pp_.ctrl_pt_dist / pp_.max_vel_;
        vector<Eigen::Vector3d> point_set, start_end_derivatives;
        KinodynamicAstar::getSamples(path, start_vel, end_vel, is_shot_succ, coef_shot, shot_time, ts, point_set, start_end_derivatives);
        Eigen::MatrixXd ctrl_pts;
        NonUniformBspline::parameterizeToBspline(
                ts, point_set, start_end_derivatives, pp_.bspline_degree_, ctrl_pts);
        NonUniformBspline init_bspline(ctrl_pts, pp_.bspline_degree_, ts);

        /*********************************
         * B-spline-based optimization   *
         *********************************/
        int cost_function = BsplineOptimizer::NORMAL_PHASE;
        if (pp_.min_time_) cost_function |= BsplineOptimizer::MINTIME;
        vector<Eigen::Vector3d> start, end;
        init_bspline.getBoundaryStates(2, 0, start, end);
        pos_traj_bspline_optimizer->setBoundaryStates(start, end);
        if (time_lb > 0) pos_traj_bspline_optimizer->setTimeLowerBound(time_lb);
        pos_traj_bspline_optimizer->optimize(ctrl_pts, ts, cost_function, 1, 1);
        local_data_->pos_traj_.setUniformBspline(ctrl_pts, pp_.bspline_degree_, ts);

2.
`FastPlannerManager::planExploreTraj`
（当前视点与下一个视点距离超限）

(1) 获得路点信息

if (tour.empty()) ROS_ERROR("Empty path to traj planner");

        // Generate traj through waypoints-based method
        const size_t pt_num = tour.size();
        //-----------------------------------------
        std::cout<<"路点数量 = "<<pt_num<<std::endl;

        Eigen::MatrixXd pos(pt_num, 3);
        for (Eigen::Index i = 0; i < pt_num; ++i) pos.row(i) = tour[i];

        Eigen::Vector3d zero(0, 0, 0);
        Eigen::VectorXd times(pt_num - 1);

（2）计算分配时间

// 时间使用路点位置信息与最大速度进行计算
        for (Eigen::Index i = 0; i < pt_num - 1; ++i)
            times(i) = (pos.row(i + 1) - pos.row(i)).norm() / (pp_.max_vel_ * 0.5);

（3）由path计算轨迹traj

  PolynomialTraj init_traj;
        PolynomialTraj::waypointsTraj(pos, cur_vel, zero, cur_acc, zero, times, init_traj);

（4）B样条优化

// B-spline-based optimization
        vector<Vector3d> points, boundary_deri;
        double duration = init_traj.getTotalTime();
        int seg_num = init_traj.getLength() / pp_.ctrl_pt_dist;
        seg_num = max(8, seg_num);                       // 最多分成8段？
        double dt = duration / double(seg_num);

3.
`FastPlannerManager::planYawExplore`
（yaw角规划）

特殊

：对yaw角进行了单独的规划

（1）规划信息

const int seg_num = 12;              
        double dt_yaw = duration / seg_num;  // time of B-spline segment
        Eigen::Vector3d start_yaw3d = start_yaw;
        std::cout << "[planner manager] B-spline时间间隔: dt_yaw: " << dt_yaw << ", start yaw: " << start_yaw3d.transpose()
                  << ", end_yaw: " << end_yaw << std::endl;

其中特别的是固定了yaw角规划的分段信息，固定为了12段，而
planExploreTraj
中轨迹最多分为8段

（2）控制yaw角方向

  while (start_yaw3d[0] < -M_PI) start_yaw3d[0] += 2 * M_PI;
        while (start_yaw3d[0] > M_PI) start_yaw3d[0] -= 2 * M_PI;
        double last_yaw = start_yaw3d[0];

（3）生成yaw角控制点

 // Yaw traj control points
        Eigen::MatrixXd yaw(seg_num + 3, 1);
        yaw.setZero();

（4）起点终点状态

// Initial state
        Eigen::Matrix3d states2pts;
        states2pts << 1.0, -dt_yaw, (1 / 3.0) * dt_yaw * dt_yaw,
                1.0, 0.0, -(1 / 6.0) * dt_yaw * dt_yaw,
                1.0, dt_yaw, (1 / 3.0) * dt_yaw * dt_yaw;
        yaw.block<3, 1>(0, 0) = states2pts * start_yaw3d;

     // Final state
        Eigen::Vector3d end_yaw3d(end_yaw, 0, 0);
        roundYaw(last_yaw, end_yaw3d(0));
        yaw.block<3, 1>(seg_num, 0) = states2pts * end_yaw3d;

（5）加入路点约束，确认前方是否可行

// Add waypoint constraints if looking forward is enabled
        vector<Eigen::Vector3d> waypts;
        vector<int> waypt_idx;
        const double forward_t = 2.0;
        const int relax_num = relax_time / dt_yaw;               //relax_time = 1.0
        for (int i = 1; i < seg_num - relax_num; ++i) {
            double tc = i * dt_yaw;
            Eigen::Vector3d pc = pos_traj.evaluateDeBoorT(tc);
            double tf = min(duration, tc + forward_t);
            Eigen::Vector3d pf = pos_traj.evaluateDeBoorT(tf);
            Eigen::Vector3d pd = pf - pc;
            Eigen::Vector3d waypt;
            if (pd.norm() > 1e-3) {
                waypt(0) = atan2(pd(1), pd(0));
                waypt(1) = waypt(2) = 0.0;
                roundYaw(last_yaw, waypt(0));
            } else
                waypt = waypts.back();

            last_yaw = waypt(0);
            waypts.push_back(waypt);
            waypt_idx.push_back(i);
        }

（6）yaw角角度规划是否变化太大

  // Debug rapid change of yaw
        if (fabs(start_yaw3d[0] - end_yaw3d[0]) >= M_PI) {
            ROS_ERROR("Yaw change rapidly!");
            // std::cout << "start yaw: " << start_yaw3d[0] << ", " << end_yaw3d[0] << std::endl;
            std::cout << "yaw变化过大!! change of yaw = " << fabs(start_yaw3d[0] - end_yaw3d[0]) <<std::endl;
        }

当超过
M_PI
时显示ROS_ERROR

(7) yaw角使用B样条优化

       // Call B-spline optimization solver
        int cost_func = BsplineOptimizer::SMOOTHNESS | BsplineOptimizer::START |
                        BsplineOptimizer::END | BsplineOptimizer::WAYPOINTS;
        vector<Eigen::Vector3d> start = {Eigen::Vector3d(start_yaw3d[0], 0, 0),
                                         Eigen::Vector3d(start_yaw3d[1], 0, 0), Eigen::Vector3d(start_yaw3d[2], 0, 0)};
        vector<Eigen::Vector3d> end = {Eigen::Vector3d(end_yaw3d[0], 0, 0), Eigen::Vector3d(0, 0, 0)};
        yaw_traj_bspline_optimizer->setBoundaryStates(start, end);
        yaw_traj_bspline_optimizer->setWaypoints(waypts, waypt_idx);
        yaw_traj_bspline_optimizer->optimize(yaw, dt_yaw, cost_func, 1, 1);

（8）将yaw的信息更新至轨迹信息

   // Update traj info
        local_data_->yaw_traj_.setUniformBspline(yaw, 3, dt_yaw);

原文链接：https://blog.csdn.net/hongliyu_lvliyu/article/details/131889012

一、循环

二、规划部分

1. FastPlannerManager::kinodynamicReplan （当前视点与下一个视点距离不超限）

2. FastPlannerManager::planExploreTraj （当前视点与下一个视点距离超限）

3. FastPlannerManager::planYawExplore （yaw角规划）

你可能也喜欢

1.
`FastPlannerManager::kinodynamicReplan`
（当前视点与下一个视点距离不超限）

2.
`FastPlannerManager::planExploreTraj`
（当前视点与下一个视点距离超限）

3.
`FastPlannerManager::planYawExplore`
（yaw角规划）