0 專欄介紹

🔥附C++/Python/Matlab全套代碼🔥課程設(shè)計、畢業(yè)設(shè)計、創(chuàng)新競賽必備！詳細介紹全局規(guī)劃(圖搜索、采樣法、智能算法等)；局部規(guī)劃(DWA、APF等)；曲線優(yōu)化(貝塞爾曲線、B樣條曲線等)。

🚀詳情：圖解自動駕駛中的運動規(guī)劃(Motion Planning)，附幾十種規(guī)劃算法

---

1 引言

在《運動規(guī)劃實戰(zhàn)精講》系列中，我們介紹了非常多種路徑規(guī)劃算法，不同的算法有不同的工作原理及其適用場景。例如，A*算法通過啟發(fā)式方法進行路徑搜索，能夠在較小的搜索空間內(nèi)找到較優(yōu)解，而RRT算法則通過隨機采樣快速擴展搜索樹，適合高維空間的路徑規(guī)劃。

自然地，我們希望能夠有一種方式定性、定量地對比不同算法的性能，比如計算效率、資源消耗等。本節(jié)通過設(shè)計一個可視化插件的形式，幫助大家進行規(guī)劃算法的對比實驗，并提供了常用的定量指標計算方法，最終效果如下所示

2 禁用局部規(guī)劃器

在ROS的導航功能包集navigation中提供了move_base功能包，用于實現(xiàn)導航功能。move_base功能包提供了基于動作(action)的路徑規(guī)劃實現(xiàn)，move_base可以根據(jù)給定的目標點，控制機器人底盤運動至目標位置，并且在運動過程中會連續(xù)反饋機器人自身的姿態(tài)與目標點的狀態(tài)信息。我們知道，move_base主要由全局路徑規(guī)劃與本地路徑規(guī)劃組成。

如果希望在給定的起點和終點對比不同的規(guī)劃算法，就需要臨時屏蔽local_planner的功能，防止機器人因為運動造成重規(guī)劃。局部規(guī)劃插件的詳細設(shè)計流程在ROS從入門到精通5-5：局部路徑規(guī)劃插件開發(fā)案例(以DWA算法為例)，這里直接給出靜態(tài)局部規(guī)劃器的代碼

bool StaticPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)
{if (!initialized_){ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");return false;}return true;
}bool StaticPlanner::isGoalReached()
{if (!initialized_){ROS_ERROR("Static planner has not been initialized");return false;}if (goal_reached_){ROS_INFO("GOAL Reached!");return true;}return false;
}bool StaticPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{if (!initialized_){ROS_ERROR("Static planner has not been initialized");return false;}cmd_vel.linear.x = 0.0;cmd_vel.angular.z = 0.0;return true;
}

3 路徑規(guī)劃定性對比實驗

3.1 加載路徑規(guī)劃器和可視化插件

首先打開src/sim_env/config中的planner.yaml文件，配置可選的規(guī)劃器屬性，如下所示

planner:- a_star- dijkstra- gbfs- jps- d_star- lpa_star- d_star_lite- voronoi- theta_star- lazy_theta_star- rrt- rrt_star- rrt_connect- informed_rrt- aco- pso- ga

接著啟動項目，在rviz中添加RMPV可視化插件

3.2 設(shè)置起點和終點

通過RMPV面板的Start Pose和Goal Pose選項設(shè)置起點和終點位姿

3.3 選擇規(guī)劃器規(guī)劃

在RMPV的Planner選項中選擇具體的規(guī)劃器，點擊Start Planning即可開始規(guī)劃，左側(cè)面板將顯示規(guī)劃器名稱、起點、終點、路徑長度、顏色等信息

3.4 不同規(guī)劃器對比

重復3.3的步驟進行不同規(guī)劃器的規(guī)劃實驗

3.5 路徑保存和加載

點擊Save Paths可以以json格式保存路徑到本地，如圖所示。再下一次實驗中，可以通過Load Paths復現(xiàn)之前的實驗

4 路徑規(guī)劃定量對比實驗

4.1 計算規(guī)劃耗時

耗時指標用cal_time.count()表示

#include <chrono>auto start_time = std::chrono::high_resolution_clock::now();
g_planner_->plan(...);
auto finish_time = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> cal_time = finish_time - start_time;
R_INFO << "Calculation Time: " << cal_time.count();

4.2 計算規(guī)劃長度

規(guī)劃長度用path_len表示

for (int i = 1; i < plan.size(); i++)
{const auto& pt1 = plan[i - 1];const auto& pt2 = plan[i];path_len += std::hypot(pt1.x() - pt2.x(), pt1.y() - pt2.y());
}
R_INFO << "Path Length: " << path_len;

4.3 計算拓展節(jié)點數(shù)

對于圖搜索算法，拓展節(jié)點數(shù)是重要指標，用expand.size()表示

path_found = g_planner_->plan({ g_start_x, g_start_y, tf2::getYaw(start.pose.orientation) },{ g_goal_x, g_goal_y, tf2::getYaw(goal.pose.orientation) }, origin_path, expand);
R_INFO << "Expand Size: " << expand.size();

4.4 計算路徑曲率

平均曲率和最大曲率是衡量路徑平滑度的重要指標，計算如下

double max_curv = 0.0;
double avg_curv = 0.0;auto calCurv = [](const PathPlanner::Point3d& pt1, const PathPlanner::Point3d& pt2,const PathPlanner::Point3d& pt3) {double ax = pt1.x();double ay = pt1.y();double bx = pt2.x();double by = pt2.y();double cx = pt3.x();double cy = pt3.y();double a = std::hypot(bx - cx, by - cy);double b = std::hypot(cx - ax, cy - ay);double c = std::hypot(ax - bx, ay - by);double cosB = (a * a + c * c - b * b) / (2 * a * c);double sinB = std::sin(std::acos(cosB));double cross = (bx - ax) * (cy - ay) - (by - ay) * (cx - ax);return std::copysign(2 * sinB / b, cross);
};for (int i = 2; i < plan.size(); i++)
{int n = i - 1;const auto& pt1 = plan[i - 2];const auto& pt2 = plan[i - 1];const auto& pt3 = plan[i];double curv = std::fabs(calCurv(pt1, pt2, pt3));max_curv = std::max(curv, max_curv);avg_curv = ((n - 1) * avg_curv + curv) / n;
}

4.5 計算路徑轉(zhuǎn)角

平均轉(zhuǎn)角和最大轉(zhuǎn)角是衡量路徑平滑度的重要指標，計算如下

double max_angle = 0.0;
double avg_angle = 0.0;auto calAngle = [](const PathPlanner::Point3d& pt1, const PathPlanner::Point3d& pt2,const PathPlanner::Point3d& pt3) {
rmp::common::geometry::Vec2d vec1(pt2.x() - pt1.x(), pt2.y() - pt1.y());
rmp::common::geometry::Vec2d vec2(pt3.x() - pt2.x(), pt3.y() - pt2.y());
return std::acos(vec1.innerProd(vec2) / (vec1.length() * vec2.length()));
};for (int i = 2; i < plan.size(); i++)
{int n = i - 1;const auto& pt1 = plan[i - 2];const auto& pt2 = plan[i - 1];const auto& pt3 = plan[i];double angle = calAngle(pt1, pt2, pt3);max_angle = std::max(angle, max_angle);avg_angle = ((n - 1) * avg_angle + angle) / n;
}

完整工程代碼請聯(lián)系下方博主名片獲取

---

🔥 更多精彩專欄：

• 《ROS從入門到精通》
• 《Pytorch深度學習實戰(zhàn)》
• 《機器學習強基計劃》
• 《運動規(guī)劃實戰(zhàn)精講》
• …

👇源碼獲取 · 技術(shù)交流 · 抱團學習 · 咨詢分享 請聯(lián)系👇