C++實作雙向RRT演算法
背景介紹
RRT（Rapidly-exploring Random Trees）是Steven M. LaValle和James J. Kuffner Jr.提出的一種通過所及構建空間搜索樹實作對非凸高維空間快速搜索演算法，該演算法可以很容易的處理包含障礙和差分運動約束的場景，因此被廣泛應用在各種機器人、無人車的運動規劃場景中，
雙向RRT演算法
為了加快隨機搜索樹規劃路徑的速度，因此提出了一種新的搜索思路，即從起點和終點同時開始構建隨機搜索樹，并每次進行判斷產生的節點是否滿足連接的條件，并在連接條件上添加了轉角約束和動態步長策略，
轉角約束是用來限制路線的轉折角度，避免超過無人車的最大轉彎角度，動態步長策略是在產生新節點時用于判斷距離障礙物的趨勢，動態的調整步長，能夠使規劃出的路徑更加平滑，同時也可加快收斂速度，
C++代碼實作如下：
具體代碼
頭檔案
//
// Created by cntia on 2022-10-01.
//
#ifndef RRT_C_RRT_H
#define RRT_C_RRT_H
#include <cmath>
#include <iostream>
using namespace std;
const int RAND_X = 21;
const int RAND_Y = 89;
const double EPS = 1e-6;
struct ListPoint{
double x;
double y;
ListPoint *parent;
ListPoint *next;
ListPoint(): x(0), y(0), parent(nullptr),next(nullptr){}
ListPoint(double x, double y): x(x), y(y), parent(nullptr), next(nullptr){}
};
struct ListObstacle {
double x;
double y;
double r;
ListObstacle *next;
ListObstacle():x(),y(), r(), next(nullptr){}
ListObstacle(double x, double y, double r):x(x),y(y), r(r), next(nullptr){}
};
struct Vector {
double x;
double y;
};
class RT {
private:
ListPoint *one;
ListPoint *two;
ListObstacle *obstacle;
ListPoint *start;
ListPoint *goal;
ListPoint *safe;
ListPoint *recover;
double angle;
double step;
double dist;
/**
* 生成隨機點
* @return
*/
static ListPoint* randomPoint();
/**
* 計算向量夾角
* @param vector1
* @param vector2
* @return
*/
double getAngle(Vector vector1, Vector vector2);
/**
* 向搜索樹插入實際新節點
* @param t
* @param point
*/
void pushBack(int t,ListPoint *point);
/**
* 查詢最近節點
* @param list
* @param point
* @return
*/
ListPoint *getNearestIndexPoint(ListPoint *list, ListPoint *point);
/**
* 查詢最近障礙物
* @param point
* @return
*/
ListObstacle *getNearestIndexObstacle(ListPoint *point);
/**
* 計算動態步長
* @param n_point
* @param a_point
* @return
*/
double dynamicStep(ListPoint *n_point, ListPoint * a_point);
/**
* 碰撞檢測
* @param t
* @param newPoint
* @return
*/
bool collisionCheck(int t,const ListPoint *newPoint);
/**
* 轉角約束檢測
* @param newPoint
* @param parentPoint
* @param ancestorPoint
* @return
*/
bool angleCheck(const ListPoint *newPoint, const ListPoint *parentPoint, const ListPoint *ancestorPoint);
/**
* 節點檢測
* @param t
* @param newPoint
* @return
*/
bool conditionCheck(int t,const ListPoint *newPoint);
/**
* 平滑連接判斷
* @param onePoint
* @param twoPoint
* @return
*/
bool perfectConnect(const ListPoint *onePoint, const ListPoint *twoPoint);
/**
* 實際坐標計算
* @param t
* @param rnd
* @return
*/
ListPoint *coordinate(int t, ListPoint *rnd);
public:
RT(ListPoint *start,ListPoint *goal,ListPoint *safe,ListPoint *recover, double angle,
double step, double dist, ListObstacle *obstacle) : start(start), goal(goal), safe(safe), recover(recover),
angle(angle), step(step),dist(dist),obstacle(obstacle) {
ListPoint *headOne = start;
headOne->next = safe;
safe->parent = headOne;
this->one = headOne;
ListPoint *headTwo = goal;
headTwo->next = recover;
recover->parent = headTwo;
this->two = headTwo;
};
/**
* 路徑規劃
*/
void planning();
};
#endif //RRT_C_RRT_H
源檔案
//
// Created by cntia on 2022-10-01.
//
#include "../headers/RRT.h"
ListPoint *RT::randomPoint() {
double x = (rand() % (RAND_Y - RAND_X + 1)) + RAND_X;
double y = (rand() % (RAND_Y - RAND_X + 1)) + RAND_X;
auto *point = new ListPoint(x, y);
return point;
}
double RT::getAngle(const Vector vector1, const Vector vector2) {
double PI = 3.141592653;
double t = (vector1.x * vector2.x + vector1.y * vector2.y) / (sqrt(pow(vector1.x, 2) + pow(vector1.y, 2)) * sqrt(pow(vector2.x, 2) + pow(vector2.y, 2)));
double angle = acos(t) * (180 / PI);
return angle;
}
void RT::pushBack(int t, ListPoint *point) {
ListPoint *last;
if(t == 1){
last = this->one;
} else {
last = this->two;
}
point->next = nullptr;
while(last->next != nullptr){
last = last->next;
}
last->next = point;
point->parent = last;
}
ListPoint *RT::getNearestIndexPoint(ListPoint *list, ListPoint *point) {
auto *minIndex = new ListPoint;
auto *head = list;
double minD = 1.79769e+308;
double d = 0;
while(head){
d = pow((point->x - head->x), 2) + pow((point->y - head->y), 2);
if(d + EPS < minD){
minD = d;
minIndex = head;
}
head = head->next;
}
return minIndex;
}
ListObstacle *RT::getNearestIndexObstacle(ListPoint *point) {
auto *minIndex = new ListObstacle;
auto *head = this->obstacle;
double minD = 1.79769e+308;
double d = 0;
while(head){
d = sqrt(pow(head->x - point->x, 2) + pow((head->y - point->y), 2)) - head->r;
if(d+EPS<minD){
minD = d;
minIndex = head;
}
head = head->next;
}
return minIndex;
}
double RT::dynamicStep(ListPoint *n_point, ListPoint *a_point) {
double theta = atan2(a_point->y - n_point->y, a_point->x - n_point->x);
a_point->x += cos(theta) * (this->dist + this->step) / 2;
a_point->y += sin(theta) * (this->dist + this->step) / 2;
auto * obstacle = getNearestIndexObstacle(a_point);
double l_n = sqrt(pow(n_point->x-obstacle->x, 2)+pow(n_point->y - obstacle->y, 2)) - obstacle->r;
double dynamic = this->step / (1 + (this->step / this->dist - 1) * exp( -3 * l_n / this->step));
return dynamic;
}
bool RT::collisionCheck(int t,const ListPoint *newPoint) {
bool flag = true;
ListObstacle *head = this->obstacle;
while(head != nullptr){
double dx = head->x - newPoint->x;
double dy = head->y - newPoint->y;
double d = sqrt(pow(dx, 2) + pow(dy, 2));
ListPoint *parentPoint = newPoint->parent;
Vector vector_p_n = {newPoint->x - parentPoint->x, newPoint->y - parentPoint->y};
Vector vector_p_o = {head->x - parentPoint->x, head->y - parentPoint->y};
double d_p_n = abs(sqrt(pow(vector_p_o.x, 2) + pow(vector_p_o.y, 2)) * sin(getAngle(vector_p_n, vector_p_o)));
if(d + EPS < head->r || d_p_n + EPS < head ->r){
flag = false;
break;
}
head = head->next;
}
return flag;
}
bool RT::angleCheck(const ListPoint *newPoint, const ListPoint *parentPoint, const ListPoint *ancestorPoint) {
Vector vector_p_n = {newPoint->x - parentPoint->x, newPoint->y - parentPoint->y};
Vector vector_a_p = {parentPoint->x - ancestorPoint->x, parentPoint->y - ancestorPoint->y};
double angle = getAngle(vector_a_p, vector_p_n);
if(angle+EPS <= this->angle){
return true;
} else{
return false;
}
}
bool RT::conditionCheck(int t,const ListPoint *newPoint) {
if(collisionCheck(t, newPoint)){
ListPoint *parentPoint = newPoint->parent;
if(parentPoint->parent == nullptr){
return false;
}
ListPoint *ancestorPoint = parentPoint->parent;
if(angleCheck(newPoint, parentPoint, ancestorPoint)){
return true;
} else {
return false;
}
} else {
return false;
}
}
bool RT::perfectConnect(const ListPoint *onePoint, const ListPoint *twoPoint) {
ListPoint *oneParent = onePoint->parent;
ListPoint *twoParent = twoPoint->parent;
Vector vector_n_w = {onePoint->x - oneParent->x, onePoint->y - oneParent->y};
Vector vector_w_x = {twoPoint->x - onePoint->x, twoPoint->y - onePoint->y};
Vector vector_x_j = {twoParent->x - twoPoint->x, twoParent->y - twoPoint->x};
double angle_one = getAngle(vector_n_w, vector_w_x);
double angle_two = getAngle(vector_w_x, vector_x_j);
if(angle_one <= this->angle - EPS){
if(fabs(angle_two - 180.0) < EPS || fabs(angle_one - 0.0) < EPS){
return false;
}else{
return true;
}
}else{
return false;
}
}
ListPoint *RT::coordinate(int t, ListPoint *rnd) {
// 尋找最近節點
auto *nearestPoint = new ListPoint;
if(t==1) {
nearestPoint = getNearestIndexPoint(this->one, rnd);
} else {
nearestPoint = getNearestIndexPoint(this->two, rnd);
}
// 按照原始步長計算虛坐標
double theta = atan2(rnd->y - nearestPoint->y, rnd->x - nearestPoint->x);
auto *newPoint = new ListPoint(nearestPoint->x + cos(theta) * this->step, nearestPoint->y + sin(theta) * this->step);
// 使用動態步長計算實際坐標
double actualStep = dynamicStep(nearestPoint, newPoint);
newPoint->x = nearestPoint->x + cos(theta) * actualStep;
newPoint->y = nearestPoint->y + sin(theta) * actualStep;
newPoint->parent = nearestPoint;
return newPoint;
}
void RT::planning() {
while(true){
ListPoint *rnd = randomPoint();
ListPoint *newPoint=coordinate(1, rnd);
if(!conditionCheck(1, newPoint)){
continue;
}
pushBack(1, newPoint);
ListPoint *newPointTwo = coordinate(2, newPoint);
if(!conditionCheck(2, newPointTwo)){
continue;
}
pushBack(2, newPointTwo);
double dx = newPoint->x - newPointTwo->x;
double dy = newPoint->y - newPointTwo->y;
double d = sqrt(pow(dx, 2)+ pow(dy, 2));
if(this-> dist+ EPS < d && d + EPS <this->step){
if(perfectConnect(newPoint, newPointTwo)){
break;
}
else{
continue;
}
}else{
continue;
}
}
ListPoint *tempOne = this->one;
while(tempOne!= nullptr){
cout<<tempOne->x<<" "<<tempOne->y<<endl;
tempOne = tempOne->next;
}
ListPoint *tempTwo = this->two;
while(tempTwo != nullptr){
cout<<tempTwo->x<<" "<<tempTwo->y<<endl;
tempTwo = tempTwo->next;
}
}
主程式
#include "headers//RRT.h"
using namespace std;
const double ANGLE = 60.0;
const double STEP = 10.0;
const double DISTANCE = 5.0;
int main() {
double obstacle_x[]= {50, 50, 50};
double obstacle_y[]= {50, 13, 87};
double obstacle_r[]= {15, 12, 11};
auto * obstacle = new ListObstacle(50, 50, 15);
for(int i = 1; i<3; i++){
auto *node = new ListObstacle;
node->x = obstacle_x[i];
node->y = obstacle_y[i];
node->r = obstacle_r[i];
node->next = obstacle->next;
obstacle->next = node;
}
auto *start = new ListPoint(0, 0);
auto *goal = new ListPoint(100, 100);
auto *safe = new ListPoint(20, 20);
auto *recover = new ListPoint(90, 90);
RT rrt = RT(start, goal, safe, recover, ANGLE, STEP, DISTANCE, obstacle);
rrt.planning();
}
轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/510866.html
標籤：其他