一、循環(huán)

• 執(zhí)行 fast_exploration_manager.cpp中的 FastExplorationManager::planExplore，進(jìn)行探索并選擇視點(diǎn)；
  此時(shí)終端打印max_id，min_id，代價(jià)cost mat以及旅行商算法參數(shù) TSP
  — 相當(dāng)于對(duì)應(yīng)原文的第一部分，建立FIS，找到邊界簇并不斷更新信息，找到視點(diǎn)

• 執(zhí)行 fast_exploration_manager.cpp中的 FastExplorationManager::planMotion ：
  其中主要調(diào)用了兩個(gè)函數(shù) kinodynamicReplan , planExploreTraj和planYawExplore ，位于planner_manager.cpp
  其中當(dāng)前視點(diǎn)與下一個(gè)視點(diǎn)距離不超過上下限時(shí)，調(diào)用 kinodynamicReplan 得到規(guī)劃路徑；超過上下限時(shí)，調(diào)用 planExploreTraj 得到規(guī)劃路徑。
  — 對(duì)應(yīng)原文第二部分，根據(jù)視點(diǎn)進(jìn)行規(guī)劃得到最終軌跡

    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]);                        //計(jì)算預(yù)期yaw角差距的絕對(duì)值
        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) {       //檢查到下一個(gè)視點(diǎn)是否有障礙物
            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;            // 如果視點(diǎn)距離大于far，則只取 far的距離
        const double radius_close = 1.5;          //原1.5  如果視點(diǎn)距離小于close則不使用kino，直接使用兩個(gè)視點(diǎn)的信息進(jìn)行優(yōu)化得到軌跡
        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范圍內(nèi)規(guī)劃軌跡
        } 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進(jìn)行規(guī)劃，沒有規(guī)劃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();
        //單獨(dú)對(duì)yaw角進(jìn)行了規(guī)劃
        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;
    }

即三種情形下的規(guī)劃分別為 ：
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;

• 終端輸出結(jié)果：（自己加了一些其他輸出打?。?/li>

&&&&&&&&&&&&&&&&&& planExplore 進(jìn)行探索，選擇視點(diǎn) &&&&&&&&&&&&&&&&&&&&
[ 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 &&&&&&&&&&&&&&&&&&&&
=================  開始執(zhí)行kino,獲得kino起點(diǎn)終點(diǎn)信息==================
-- 在kino中調(diào)用A* search --
--------------KinodynamicAstar begin to search ----------------------
vel:-0.0602589, acc:-0.891468
near end
[non uniform B-spline] A size	120
============== B樣條規(guī)劃 yaw角  ===================
[planner manager] B-spline時(shí)間間隔: 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 進(jìn)行探索，選擇視點(diǎn) &&&&&&&&&&&&&&&&&&&&
[ 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樣條規(guī)劃軌跡 (x,y,z)    ===============
[planner manager]  duration: 5.81806, seg_num: 8, dt: 0.727257
[non uniform B-spline] A size	143
============== B樣條規(guī)劃 yaw角  ===================
[planner manager] B-spline時(shí)間間隔: 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 =======================================

二、規(guī)劃部分

1. FastPlannerManager::kinodynamicReplan （當(dāng)前視點(diǎn)與下一個(gè)視點(diǎn)距離不超限）

    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開始規(guī)劃 =================="<<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 << "起點(diǎn)終點(diǎn)距離過近, Close goal" << endl;
            return false;
        }

        /******************************
         * Kinodynamic path searching *
         ******************************/
        vector<PathNodePtr> path;
        double shot_time;
        Matrix34 coef_shot;
        bool is_shot_succ;
        std::cout << "-- 在kino中調(diào)用A* search 驗(yàn)證--" << std::endl;
        //最多使用search規(guī)劃兩次
        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;
            }
        }

其中函數(shù)search主要用于尋找路徑，輸入起點(diǎn)的p,v,a,終點(diǎn)p,v，布爾值init_search，路點(diǎn)path，布爾值is_shot_succ，矩陣coef_shot，時(shí)間shot_time；當(dāng)?shù)谝淮握也坏杰壽E時(shí)布爾值布爾值init_search置為false，會(huì)再次調(diào)用：

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

對(duì)應(yīng)函數(shù)位于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的主要影響：
  當(dāng)為true時(shí)，tau的計(jì)算方法不一樣

        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開始規(guī)劃 ==================
[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中調(diào)用A* search 驗(yàn)證--
--------------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

最后對(duì)路徑進(jìn)行B樣條優(yōu)化：

     /*********************************
         * 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 （當(dāng)前視點(diǎn)與下一個(gè)視點(diǎn)距離超限）

(1) 獲得路點(diǎn)信息

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<<"路點(diǎn)數(shù)量 = "<<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）計(jì)算分配時(shí)間

// 時(shí)間使用路點(diǎn)位置信息與最大速度進(jìn)行計(jì)算
        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計(jì)算軌跡traj

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

（4）B樣條優(yōu)化

// 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角規(guī)劃）

特殊：對(duì)yaw角進(jìn)行了單獨(dú)的規(guī)劃

（1）規(guī)劃信息

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時(shí)間間隔: dt_yaw: " << dt_yaw << ", start yaw: " << start_yaw3d.transpose()
                  << ", end_yaw: " << end_yaw << std::endl;

其中特別的是 固定了yaw角規(guī)劃的分段信息，固定為了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角控制點(diǎn)

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

（4）起點(diǎn)終點(diǎn)狀態(tài)

// 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）加入路點(diǎn)約束，確認(rèn)前方是否可行

// 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角角度規(guī)劃是否變化太大

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

當(dāng)超過M_PI時(shí)顯示ROS_ERROR

(7) yaw角使用B樣條優(yōu)化

       // 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的信息更新至軌跡信息文章來源地址http://www.zghlxwxcb.cn/news/detail-630302.html

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

到了這里，關(guān)于無人機(jī)自主探索FUEL：代碼閱讀3--執(zhí)行循環(huán)順序與部分釋義的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！