ROS 一種基於RRT的路徑規劃器

最近寫了乙個基於rrt的全域性路徑規劃器在此記錄下來。

ubuntu18.04

ros-melodic

作為move_base外掛程式。

包括rrt* ，rrt-connect，啟發式擴充套件，路徑優化等內容。

//過載！=

};（一般為機械人起始點和目標點）然後規劃這兩個點之間的路徑。函式為：

bool rrtstarplannerros::
makeplan
(const geometry_msgs::posestamped& start,
const geometry_msgs::posestamped& goal,
std::vector<:posestamped>
& plan)

plan即為規劃出的路徑。

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 node
start_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 node
goal_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; //用於儲存距離當前節點最近的節點

1、首先用隨機數判斷是隨機擴充套件還是啟發式擴充套件，0.8的概率使用隨機取樣擴充套件，0.2的概率使用啟發擴充套件。隨機取樣意思是隨機在地圖範圍內生成xy座標且該xy座標點為可行區域。啟發擴充套件意思是本次的隨機節點 = 目標點然後進行後面的計算。

srand
(ros::time::
now().
tonsec()
+ seed++);
//修改種子
unsigned
int rand_nu =
rand()
%10;if
(rand_nu >1)
// 0.8的概率使用隨機取樣擴充套件
else
// 0.2的概率使用啟發擴充套件

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

node_nearest =
getnearest
(nodes, p_rand)
;

3、從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為擴充套件出來的新節點。

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

if
(obstaclefree
(node_nearest, p_new.first, p_new.second)
)break
;}

5、如果樹一與樹二連線到了一起則得出路徑

if
(is_connect_to_tree2)

6、如果樹一擴充套件到了目標點足夠近則得出路徑。

// 第一棵樹搜尋到目標點if(
pointcirclecollision
(p_new.first, p_new.second, goal.pose.position.x , goal.pose.position.y, goal_radius_)
)

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

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

首先安裝turtlebot3_fake和turtlebot3_bringup

sudo apt-get install ros-melodic-turtlebot3-fake sudo apt-get install ros-melodic-turtlebot3-bringup

然後啟動rrt節點的launc**件

roslaunch rrt_star_global_planner rrt_node.launch

在rviz上傳送目標點即可。

設定引數：base_global_planner: rrtstar_planner/rrtstarplannerros ,

然後movebase載入/params/rrt_star_planner.yaml引數即可。

ROS 一種基於RRT的路徑規劃器

基於unity3d的RRT演算法路徑規劃

ROS 一種路徑優化方法拉直法

一種DTO的規劃方案

ROS 一種基於RRT的路徑規劃器

基於unity3d的RRT演算法路徑規劃

ROS 一種路徑優化方法 拉直法

一種DTO的規劃方案

相關推薦

ROS 一種路徑優化方法拉直法