无人机自主探索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);