1.介紹

快速搜索隨機樹算法（Rapidly-exploring Random Tree，RRT）由Lavalle提出，是一種采用增量方式增長的隨機采樣算法，用于解決有代數約束（障礙帶來的）和微分約束（非完整性和動態環境帶來的）的高維空間問題。

RRT算法的優勢在于無需對系統進行建模，無需對搜索區域進行幾何劃分，在搜索空間的覆蓋率高，搜索的范圍廣，可以盡可能的探索未知區域。但同時也存在算法計算代價過大的問題。

RRT* 與基本RRT算法的主要區別在于RRT* 算法引入了對新生成節點相鄰節點的搜索，目的是選擇低代價的父節點，除此之外還有重新布線的過程進一步減小路徑代價，是解決高維的最優路徑規劃問題的一個突破性的方法。

RRT* 算法是漸進最優的，該算法在原有的RRT算法上，改進了父節點選擇的方式，采用代價函數來選取拓展節點領域內最小代價的節點為父節點，同時，每次迭代后都會重新連接現有樹上的節點，從而保證計算的復雜度和漸進最優解。

2.代碼鏈接

GitHub - li-huanhuan/rrt_star_global_planner

3.效果

4.作為插件加入move_base

<param name="base_global_planner" value="RRTstar_planner/RRTstarPlannerROS"/> <rosparam file="$(find 功能包名)/文件路徑/base_global_planner_params.yaml" command="load" />

?base_global_planner_params.yaml

RRTstarPlannerROS:search_radius: 2.0 #搜索附近的節點的搜索范圍goal_radius: 0.2 #認為搜索到目標點的范圍epsilon_min: 0.001 #節點之間的最小允許距離epsilon_max: 0.1 #節點之間的最大允許距離max_nodes_num: 2000000000.0 #節點數的最大值，最大迭代次數plan_time_out: 10.0 #規劃超時，默認10s

5.部分代碼實現過程

????????0.節點的定義

struct Node {double x; //節點坐標xdouble y; //節點坐標yint node_id; //節點idint parent_id; //父節點iddouble cost; //節點代價，可以是柵格地圖的代價值bool operator ==(const Node& node) //重載=={return (fabs(x - node.x) < 0.0001) && (fabs(y - node.y) < 0.0001) && (node_id == node.node_id) && (parent_id == node.parent_id) && (fabs(cost - node.cost) < 0.0001) ;}bool operator !=(const Node& node) //重載！={if((fabs(x - node.x) > 0.0001) || (fabs(y - node.y) > 0.0001) || (node_id != node.node_id) || (parent_id != node.parent_id) || (fabs(cost - node.cost) > 0.0001))return true;elsereturn false;} };

? ? ? ? 1.收到兩個點

（一般為機器人起始點和目標點）然后規劃這兩個點之間的路徑。函數為：

bool RRTstarPlannerROS::makePlan(const geometry_msgs::PoseStamped& start,const geometry_msgs::PoseStamped& goal,std::vector<geometry_msgs::PoseStamped>& plan)

plan即為規劃出的路徑。

? ? ? ? 2.定義兩棵樹

std::vector< Node > nodes; //第一棵樹，從起始點向終點擴散 std::vector< Node > nodes_2; //第二棵樹，從終點向起始點擴散

起始點轉換成節點并放入樹1

Node start_node;start_node.x = start.pose.position.x;start_node.y = start.pose.position.y;start_node.node_id = 0;start_node.parent_id = -1; // None parent nodestart_node.cost = 0.0;nodes.push_back(start_node);

終點轉換成節點并放入樹2

Node goal_node;goal_node.x = goal.pose.position.x;goal_node.y = goal.pose.position.y;goal_node.node_id = 0;goal_node.parent_id = -1; // None parent nodegoal_node.cost = 0.0;nodes_2.push_back(goal_node);

定義一些中間變量供計算使用

std::pair<double, double> p_rand; //隨機采樣的可行點std::pair<double, double> p_new; //第一棵樹的新節點std::pair<double, double> p_new_2; //第二棵樹的新節點Node connect_node_on_tree1; //第二課樹與第一課樹連接到一起時第一課樹上距離第二課樹最近的節點Node connect_node_on_tree2; //第一課樹與第二課樹連接到一起時第二課樹上距離第二課樹最近的節點bool is_connect_to_tree1 = false; //第二課樹主動連接到第一課樹上標志bool is_connect_to_tree2 = false; //第一課樹主動連接到第二課樹上標志Node node_nearest; //用于存儲距離當前節點最近的節點

? ? ? ? 3.開始擴展樹一

首先用隨機數判斷是隨機擴展還是啟發式擴展，0.8的概率使用隨機采樣擴展，0.2的概率使用啟發擴展。隨機采樣意思是隨機在地圖范圍內生成xy坐標且該xy坐標點為可行區域。啟發擴展意思是本次的隨機節點 = 目標點 然后進行后面的計算。

srand(ros::Time::now().toNSec() + seed++);//修改種子unsigned int rand_nu = rand()%10;if(rand_nu > 1) // 0.8的概率使用隨機采樣擴展{p_rand = sampleFree(); // random point in the free space}else // 0.2的概率使用啟發擴展{p_rand.first = goal.pose.position.x;p_rand.second = goal.pose.position.y;}

p_rand為一個新的隨機采樣點，從樹一中找到距離p_rand最近的一個節點

node_nearest = getNearest(nodes, p_rand);

從node_nearest（距離本次隨機節點最近的節點）朝向本次隨機節點擴展出一個新節點p_new，擴展的長度有距離限制，epsilon_min_為最小距離限制，epsilon_max_為最大距離限制。

p_new = steer(node_nearest.x, node_nearest.y, p_rand.first, p_rand.second);

p_new為擴展出來的新節點。

判斷新擴展出來的節點（p_new）與最近的節點（node_nearest）之間是否有障礙物，若無障礙物則執行以下操作否則返回步驟1。

if (obstacleFree(node_nearest, p_new.first, p_new.second)){//樹枝無碰撞Node newnode;newnode.x = p_new.first;newnode.y = p_new.second;newnode.node_id = nodes.size(); // index of the last element after the push_bask belownewnode.parent_id = node_nearest.node_id;newnode.cost = 0.0;// 優化newnode = chooseParent(node_nearest, newnode, nodes); // Select the best parentnodes.push_back(newnode);rewire(nodes, newnode); 優化新節點附近的節點，選擇最優的父節點geometry_msgs::Point point_tem;point_tem.x = nodes[newnode.parent_id].x;point_tem.y = nodes[newnode.parent_id].y;point_tem.z = 0;this->marker_tree_.points.push_back(point_tem);if(this->isConnect(newnode,nodes_2,nodes, connect_node_on_tree2)) //判斷樹一是否與樹二連接到一起{is_connect_to_tree2 = true;}break;}

如果樹一與樹二連接到了一起則得出路徑

if(is_connect_to_tree2){std::cout << "兩棵樹連接在了一起 1->2 耗時：" << ros::Time::now().toSec() - start_time << "秒" << std::endl;getPathFromTree(nodes,nodes_2,connect_node_on_tree2,plan,GetPlanMode::CONNECT1TO2);plan[0].pose.orientation = start.pose.orientation;plan[plan.size()-1].pose.orientation = goal.pose.orientation;nav_msgs::Path path_pose;path_pose.header.frame_id = this->frame_id_;path_pose.header.stamp = ros::Time::now();path_pose.poses = plan;plan_pub_.publish(path_pose);return true;}

如果樹一擴展到了目標點足夠近則得出路徑。

// 第一棵樹搜索到目標點if (pointCircleCollision(p_new.first, p_new.second, goal.pose.position.x , goal.pose.position.y, goal_radius_) ){std::cout << "第一棵樹搜索到目標點,耗時：" << ros::Time::now().toSec() - start_time << "秒" << std::endl;getPathFromTree(nodes,nodes_2,nodes.back(),plan,GetPlanMode::TREE1);plan[0].pose.orientation = start.pose.orientation;plan[plan.size()-1].pose.orientation = goal.pose.orientation;nav_msgs::Path path_pose;path_pose.header.frame_id = this->frame_id_;path_pose.header.stamp = ros::Time::now();path_pose.poses = plan;plan_pub_.publish(path_pose);return true;}

? ? ? ? 4.開始擴展樹二

與第一課樹的擴展區別：第二棵樹的隨機節點 = 第一棵樹的新節點，其他步驟與樹一的擴展步驟類似。

// 第二棵樹p_rand.first = p_new.first;p_rand.second = p_new.second;

總結

以上是生活随笔為你收集整理的基于RRT的路径规划器的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。