[源码阅读]SMUG Planner

admin 管理员组

文章数量: 887007

[源码阅读]SMUG Planner

A Safe Multi-Goal Planner for Mobile Robots in Challenging Environments

动态规划算法实现函数

1.从起点开始 连接 起点 到 最后一个TOI的所有 POI 并计算路径代价，

2.然后转入到 上一个 TOI （倒数第二个），计算新的 TOI 到刚刚的TOI 内所有 POI 之间的路径 以及代价，

3.计算 当前TOI 每一个POI j 到起点时经过的上一个 TOI 的第k 个POI ，记录下路径代价最小时候的 j 对应的k ，放入一个二维查找表，

4.这个二维查找表的行对应的是一个TOI ，列对应的是 这个 TOI 里第 j 个POI 想连接到起点时候，经过的上一个 TOI 的第k 个 POI ，这个k 使路径代价最小。

5.重复二三四，即得到最终的查找表

6.最后根据查找表得到访问顺序的路径点POI们

7.首先计算起点到第一个TOI 的所有的POI的路径以及代价，找到路径代价最小的对应的第k个POI;

8.因为已经直到了终点也就是起点，到达第一个TOI的最短路径对应的j k POI了，也就是那个查找表，所以根据第7步找到的k 在查找表回找就可以找到起点经过k POI 到终点（起点）的一个路径了，代价最小。

9.再封闭路径。

PlannerStatus Planner::dynamicProgramming(const std::vector<std::vector<ob::ScopedState<>>>& goal_sets, // TOIS 和 POIS 每一个 vector 的第一个是 TOIstd::vector<ob::ScopedState<>> points) { // 所有的 POIint n_points = points.size(); // POIs numint n_set = goal_sets.size(); // TOIS numog::PathGeometricPtr** path_matrix; // 符合几何约束的路径矩阵path_lengths_ = new double*[n_points];path_matrix = new og::PathGeometricPtr*[n_points];for (int i = 0; i < n_points; i++) {path_lengths_[i] = new double[n_points];path_matrix[i] = new og::PathGeometricPtr[n_points];}// dynamic programming and plannning during stepsauto center_sequence_temp = center_sequence_; // center_sequence_ TSP 求解的 TOI 的顺序 第一个应该是 startstd::vector<double> cost_vector; // 起点 start 到 每一个 POI 的最短路径 代价// the last one goal to startint cur_center = center_sequence_temp.back(); //TODO 最后访问的 TOI 还是 第一个访问的 TOI center_sequence_temp.pop_back();std::vector<std::vector<int>> next_point_table(n_set - 1);int next_point = n_points - 1; // start pointfor (int i = 0; i < goal_sets[cur_center].size() - 1; i++) { // 应该不包括 TOI if (stop_planning_) {return PREEMPTED;}int cur_point = getIdxInVector(cur_center, i); // 每一个TOI 的 POI// plan p2pPlannerStatus solved = singleGoalPlan(points[cur_point], points[next_point]);og::PathGeometric path(ss_->getSpaceInformation()); // plan 两个点(每一个到开始点)之间的路径std::shared_ptr<og::PathGeometric> path_ptr;switch (solved) {case PlannerStatus::SOLVED:path = getSingleSolutionPath();path_ptr = std::make_shared<og::PathGeometric>(path);path_matrix[cur_point][next_point] = path_ptr; // 记录每一个点到起点的路径的矩阵path_lengths_[cur_point][next_point] = path_ptr->length(); // 记录每一个点到起点的路径的长度break;default:path_lengths_[cur_point][next_point] = INFINITY;break;}cost_vector.push_back(path_lengths_[cur_point][next_point]); // 记录了当前TOI的每一个POI点到 起点 的路径长度 cost}int next_center = cur_center; // 当前TOI 变为 下一个中心TOIfor (int i = 1; i < n_set; i++) {cur_center = center_sequence_temp.back(); //TODO 最后一个TOI的上一个center_sequence_temp.pop_back();std::vector<double> cost_vector_new;std::vector<int> next_point_list;for (int j = 0; j < goal_sets[cur_center].size() - 1; j++) { // 这个 for 计算了当前中心的第j 个点经过上一个中心的第k个POI 是到达起点的最短路径代价和kif (stop_planning_) {return PREEMPTED;}int cur_point = getIdxInVector(cur_center, j); // 新的当前点double best = INFINITY;int best_idx;for (int k = 0; k < goal_sets[next_center].size() - 1; k++) { // 这个 for 找到了 cur center 的第j个POI 到 起点 经过的 上一个 中心的 第 k 个 POI 这时候路径代价最小int next_point = getIdxInVector(next_center, k);// plan p2pPlannerStatus solved = singleGoalPlan(points[cur_point], points[next_point]);og::PathGeometric path(ss_->getSpaceInformation());std::shared_ptr<og::PathGeometric> path_ptr;switch (solved) {case PlannerStatus::SOLVED:path = getSingleSolutionPath();path_ptr = std::make_shared<og::PathGeometric>(path);path_matrix[cur_point][next_point] = path_ptr;path_lengths_[cur_point][next_point] = path_ptr->length();break;default:path_lengths_[cur_point][next_point] = INFINITY;break;}double cur_cost = path_lengths_[cur_point][next_point] + cost_vector[k];if (cur_cost < best) {best = cur_cost;best_idx = k;}}cost_vector_new.push_back(best);next_point_list.push_back(best_idx); // 只保留了 每一个 j 到 k 到 起点的路径代价最小时候的，k 和最小路径代价}next_point_table[n_set - 1 - i] = next_point_list; // 每一个j 到起点对应的最小路径代价的k 的列表 放入一个二维数组next_center = cur_center; // 更新cost_vector.clear();cost_vector.insert(cost_vector.end(), cost_vector_new.begin(), cost_vector_new.end()); // 每一个j 到起点对应的最小路径代价的k 的 路径代价 放入一个二维数组}// first step from startint cur_point = n_points - 1;double best = INFINITY;int best_idx;for (int k = 0; k < goal_sets[next_center].size() - 1; k++) { // 第一个 TOIif (stop_planning_) {return PREEMPTED;}int next_point = getIdxInVector(next_center, k);// plan p2pPlannerStatus solved = singleGoalPlan(points[cur_point], points[next_point]); // 起点 到 第一个 TOIog::PathGeometric path(ss_->getSpaceInformation());std::shared_ptr<og::PathGeometric> path_ptr;switch (solved) {case PlannerStatus::SOLVED:path = getSingleSolutionPath();path_ptr = std::make_shared<og::PathGeometric>(path);path_matrix[cur_point][next_point] = path_ptr;path_lengths_[cur_point][next_point] = path_ptr->length();break;default:path_lengths_[cur_point][next_point] = INFINITY;break;}double cur_cost = path_lengths_[cur_point][next_point] + cost_vector[k];if (cur_cost < best) {best = cur_cost;best_idx = k;}} // 起点 到 第一个 TOI 路径最短时 对应的 第 k 个 POI// the first goal is best_idxint cur_idx = best_idx;std::vector<int> dp_sequence{};for (int i = 0; i < n_set; i++) {if (i > 0) cur_idx = next_point_table[i - 1][cur_idx]; // 后面这个cur_idx  就是 best_idx 就是 第一个TOI 的k 相对于下一个 TOI 来说 就是 next_point_table 里面的 jint cur_center = center_sequence_[i];                   // next_point_table 记录了[第i个TOI中的][第j个POI]连接的下一个TOI的最佳的kint next_point = getIdxInVector(cur_center, cur_idx);dp_sequence.push_back(next_point); // 起点到终点的第 k 个 POI}int cur_index = n_points - 1; // 起点for (int next_index : dp_sequence) { // 将 dp 序列转化为 路径og::PathGeometricPtr path_ptr_temp = path_matrix[cur_index][next_index]; og::PathGeometric path_temp = *path_ptr_temp;path_buffer_ompl_.push_back(path_ptr_temp);complete_path_dp_->append(path_temp);cur_index = next_index;}// close the pathog::PathGeometricPtr path_ptr_temp = path_matrix[cur_index][n_points - 1];og::PathGeometric path_temp = *path_ptr_temp;path_buffer_ompl_.push_back(path_ptr_temp);complete_path_dp_->append(path_temp); // 最后一个 POI 连接到起点// free memoryfor (int i = 0; i < n_points; i++) {delete[] path_lengths_[i];delete[] path_matrix[i];}delete[] path_lengths_;delete[] path_matrix;return PlannerStatus::SOLVED;
}

PlannerStatus Planner::iterativeDP(const std::vector<std::vector<ob::ScopedState<>>>& goal_sets,std::vector<ob::ScopedState<>> points) {ompl::time::point start_misc5 = ompl::time::now();int n_points = points.size(); // POIs numint n_set = goal_sets.size(); // TOIs numog::PathGeometricPtr** path_matrix; // 记录POI 之间的路径path_lengths_ = new double*[n_points]; // 记录POI 之间的路径代价path_matrix = new og::PathGeometricPtr*[n_points];for (int i = 0; i < n_points; i++) {path_lengths_[i] = new double[n_points];path_matrix[i] = new og::PathGeometricPtr[n_points];}auto center_sequence_temp = center_sequence_;double best_cost = INFINITY;std::vector<int> best_point_in_set; // 最好的POIint n_planned_paths = 0; // 规划的路径数量double total_dp_time = 0; // 计算时间while (true) {// start dpompl::time::point dp_start_time = ompl::time::now();auto center_sequence_temp = center_sequence_;std::vector<double> cost_vector;// the last one goal to startint cur_center = center_sequence_temp.back();center_sequence_temp.pop_back();std::vector<std::vector<int>> next_point_table(n_set - 1); // j POI 连接 k POI 的查找表int next_point = n_points - 1; // startfor (int i = 0; i < goal_sets[cur_center].size() - 1; i++) {int cur_point = getIdxInVector(cur_center, i);if (!path_matrix[cur_point][next_point]) {// this means obstacle free path is not planned yet, use euclidean cost insteadcost_vector.push_back(points[cur_point].distance(points[next_point]));} else {// this means obstacle free path is planned, use path costcost_vector.push_back(path_lengths_[cur_point][next_point]);}}int next_center = cur_center;for (int i = 1; i < n_set; i++) {cur_center = center_sequence_temp.back();center_sequence_temp.pop_back();std::vector<double> cost_vector_new;std::vector<int> next_point_list;for (int j = 0; j < goal_sets[cur_center].size() - 1; j++) {int cur_point = getIdxInVector(cur_center, j);double best = INFINITY;int best_idx;for (int k = 0; k < goal_sets[next_center].size() - 1; k++) {int next_point = getIdxInVector(next_center, k);double cur_cost = 0;if (!path_matrix[cur_point][next_point]) {// this means obstacle free path is not planned yet, use euclidean cost insteadcur_cost = points[cur_point].distance(points[next_point]) + cost_vector[k];} else {// this means obstacle free path is planned, use path costcur_cost = path_lengths_[cur_point][next_point] + cost_vector[k];}if (cur_cost < best) {best = cur_cost;best_idx = k;}}cost_vector_new.push_back(best);next_point_list.push_back(best_idx);}next_point_table[n_set - 1 - i] = next_point_list;next_center = cur_center;cost_vector.clear();cost_vector.insert(cost_vector.end(), cost_vector_new.begin(), cost_vector_new.end());} // 同 DP 计算两两POI 之间的 COST，以及记录 j k 对应关系// first step from startint cur_point = n_points - 1; // start double best = INFINITY;int best_idx;for (int k = 0; k < goal_sets[next_center].size() - 1; k++) {double cur_cost = 0;int next_point = getIdxInVector(next_center, k);if (!path_matrix[cur_point][next_point]) {// this means obstacle free path is not planned yet, use euclidean cost insteadcur_cost = points[cur_point].distance(points[next_point]) + cost_vector[k];} else {// this means obstacle free path is planned, use path costcur_cost = path_lengths_[cur_point][next_point] + cost_vector[k];}if (cur_cost < best) {best = cur_cost;best_idx = k;}}// the first goal is best_idxint cur_idx = best_idx;std::vector<int> point_in_set(n_set);for (int i = 0; i < n_set; i++) {if (i > 0) cur_idx = next_point_table[i - 1][cur_idx];int cur_center = center_sequence_[i]; // 该去参观的第 i 个 TOIpoint_in_set[cur_center] = getIdxInVector(cur_center, cur_idx); // 参观第 i 个 TOI 的时候，应该取得那一个 POI} // 得到顺序访问的 k POIdouble dp_time = ompl::time::seconds(ompl::time::now() - dp_start_time);total_dp_time += dp_time;// TODO IDP 不同于DP的地方// 上面得到的是路径代价最小时候的访问POI 序列（包括，规划了路径和不规划的路径的POI组合）// 下面就可以通过判断有没有规划路径，没有规划就给他规划一条路径，这样可以达到这个从起点到终点的经过的所有POI 之间都规划了路径，而这个cost // 记录了这个已经规划了所有经过的POI 之间的路径的整个路径代价值// while 的下一次循环又会找到一个访问组合POI序列，下面又会给这个组合的所有POI之间进行路径规划，并记录cost.// 通过保留最小的 cost 的路径（这个路径的POI组合）就可以得到最小路径代价的访问POI 路径了。// 直到没有需要规划的POI 之间的路径为止。// obstacle free path planning according to point_in_setstd::vector<og::PathGeometric> path_before;std::vector<bool> is_planned_one_iteration;og::PathGeometric path_after = og::PathGeometric(ss_->getSpaceInformation());bool need_to_plan = false;double cost = 0.0;int last_point = n_points - 1; // startfor (int i = 0; i <= n_set; i++) {int cur_point;if (i == n_set) {cur_point = n_points - 1; // 又回到了起点} else {int cur_set = center_sequence_[i]; // 要参观的第 i 个 TOIcur_point = point_in_set[cur_set]; // 要参观的第 i 个 TOI 的第 k 个 POI}// check if path is already plannedif (!path_matrix[last_point][cur_point]) { // 如果两个POI 之间没有规划过路径need_to_plan = true; // 需要规划路径n_planned_paths++; // 规划路径计数++PlannerStatus solved = singleGoalPlan(points[last_point], points[cur_point]);og::PathGeometric path(ss_->getSpaceInformation());std::shared_ptr<og::PathGeometric> path_ptr;switch (solved) {case PlannerStatus::SOLVED:path = getSingleSolutionPath();path_ptr = std::make_shared<og::PathGeometric>(path);path_matrix[last_point][cur_point] = path_ptr;path_lengths_[last_point][cur_point] = path_ptr->length();break;default:path_lengths_[last_point][cur_point] = INFINITY;break;}path_after.append(path); // 填入规划的路径is_planned_one_iteration.push_back(false); // 没有被规划过path_before.push_back(og::PathGeometric(ss_->getSpaceInformation(), points[last_point].get(), points[cur_point].get())); // 填入由两个状态确定的一个路径实例} else {path_after.append(*path_matrix[last_point][cur_point]);is_planned_one_iteration.push_back(true); // 之前被规划过path_before.push_back(*path_matrix[last_point][cur_point]);}cost += path_lengths_[last_point][cur_point]; // 此时给一个最佳POI 组合规划了一个完整的路径，并记录了这个 路径 costlast_point = cur_point;}idp_paths_before_plan_.push_back(path_before);idp_paths_after_plan_.push_back(path_after);is_planned_.push_back(is_planned_one_iteration);if (cost < best_cost) { // 时刻记录 cost 最小的那个路径best_cost = cost;best_point_in_set = point_in_set;}if (!need_to_plan) { // 没有 POI 之间存在没有规划的路径的情况了break;}}// glue solutionint cur_index = n_points - 1;for (int next_set : center_sequence_) {int next_index = best_point_in_set[next_set];og::PathGeometricPtr path_ptr_temp = path_matrix[cur_index][next_index];og::PathGeometric path_temp = *path_ptr_temp;path_buffer_ompl_.push_back(path_ptr_temp);complete_path_idp_->append(path_temp);cur_index = next_index;}// close the pathog::PathGeometricPtr path_ptr_temp = path_matrix[cur_index][n_points - 1];og::PathGeometric path_temp = *path_ptr_temp;path_buffer_ompl_.push_back(path_ptr_temp);complete_path_idp_->append(path_temp);// free memoryfor (int i = 0; i < n_points; i++) {delete[] path_lengths_[i];delete[] path_matrix[i];}delete[] path_lengths_;delete[] path_matrix;return PlannerStatus::SOLVED;
}

本文标签： 源码阅读SMUG Planner