1.介紹

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

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

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

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

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 #搜索附近的節(jié)點的搜索范圍goal_radius: 0.2 #認為搜索到目標點的范圍epsilon_min: 0.001 #節(jié)點之間的最小允許距離epsilon_max: 0.1 #節(jié)點之間的最大允許距離max_nodes_num: 2000000000.0 #節(jié)點數(shù)的最大值，最大迭代次數(shù)plan_time_out: 10.0 #規(guī)劃超時，默認10s

5.部分代碼實現(xiàn)過程

????????0.節(jié)點的定義

struct Node {double x; //節(jié)點坐標xdouble y; //節(jié)點坐標yint node_id; //節(jié)點idint parent_id; //父節(jié)點iddouble cost; //節(jié)點代價，可以是柵格地圖的代價值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.收到兩個點

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

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

plan即為規(guī)劃出的路徑。

? ? ? ? 2.定義兩棵樹

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

起始點轉換成節(jié)點并放入樹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);

終點轉換成節(jié)點并放入樹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; //第一棵樹的新節(jié)點std::pair<double, double> p_new_2; //第二棵樹的新節(jié)點Node connect_node_on_tree1; //第二課樹與第一課樹連接到一起時第一課樹上距離第二課樹最近的節(jié)點Node connect_node_on_tree2; //第一課樹與第二課樹連接到一起時第二課樹上距離第二課樹最近的節(jié)點bool is_connect_to_tree1 = false; //第二課樹主動連接到第一課樹上標志bool is_connect_to_tree2 = false; //第一課樹主動連接到第二課樹上標志Node node_nearest; //用于存儲距離當前節(jié)點最近的節(jié)點

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

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

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的概率使用啟發(fā)擴展{p_rand.first = goal.pose.position.x;p_rand.second = goal.pose.position.y;}

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

node_nearest = getNearest(nodes, p_rand);

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

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

p_new為擴展出來的新節(jié)點。

判斷新擴展出來的節(jié)點（p_new）與最近的節(jié)點（node_nearest）之間是否有障礙物，若無障礙物則執(zhí)行以下操作否則返回步驟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;// 優(yōu)化newnode = chooseParent(node_nearest, newnode, nodes); // Select the best parentnodes.push_back(newnode);rewire(nodes, newnode); 優(yōu)化新節(jié)點附近的節(jié)點，選擇最優(yōu)的父節(jié)點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.開始擴展樹二

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

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

總結

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

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