C++結合OpenCV實現RRT算法(路徑規劃算法)

1.RRT算法簡介

代碼鏈接:RRT
動圖展示

RRT

2.算法整體框架流程

RRT算法整體框架主要分為rand、near、new三點的建立和near與new之間的安全性檢查

2.1 rand點的建立

rand點表示在地圖 M M M中隨機采樣獲得,記住是隨機。我們可以通過設計隨機函數,讓盡可能的點進入空曠區域,即算法框架中的Sample函數。下圖中紅色點表示起點,綠色的點表示終點。

2.2 near和new點的建立

near點表示從RRT樹 Γ Gamma Γ中通過距離函數,判斷樹中哪個點距離當前rand點最近,此時該點即為near點。對應於算法框架中的Near函數。

new點表示以near點到rand為方向,以 E i E_i Ei為步長,生成的一個新點。對應於算法框架的Steer函數。

2.3 安全性檢查

若上述的new點在安全區域內,且new與near點連線安全,則會在RRT樹中進行擴展,否則不會進行擴展。對應於算法框架中的CollisionFree函數。

2.4 算法結束判斷

算法框架中的當new點與goal相等,表示算法運行成功,但是實際編程情況中,new點與goal點會存在一定的距離閾值。

3.RRT代碼框架

3.1 主函數

main.cpp :首先通過地圖文件中讀取地圖數據(本次代碼提供兩張地圖,供測試使用),然後設置RRT算法的起點和終點,以及相關參數設置,例如距離閾值、步長、迭代次數等。其次通過RRT算法的接口函數RRTCoreCreatePath獲得RRT算法的路徑,最後通過顯示函數Display進行數據可視化。

#include <iostream>
#include <vector>
#include <string>
#include "map.h"
#include "display.h"
#include "RRT.h"
using namespace std;
int main()
{
	//讀取地圖點
	//vector<vector<int>>mapData = MapData("map/map.txt");
	定義起點和終點,以及閾值
	//int xStart = 10;
	//int yStart = 10;
	//int xGoal = 700;
	//int yGoal = 700;
	//int thr = 50;
	//int delta = 30;
	//int numer = 3000;
	//讀取地圖點
	vector<vector<int>>mapData = MapData("map/map6.txt");
	//定義起點和終點,以及閾值
	int xStart = 134;       //起點x值
	int yStart = 161;       //起點y值
	int xGoal = 251;        //終點x值
	int yGoal = 61;         //終點y值
	int thr = 10;           //結束與終點的距離閾值
	int delta = 10;         //步長
	int numer = 3000;       //迭代參數
	//創建RRT對象
	CRRT rrt(xStart, yStart, xGoal, yGoal, thr, delta, mapData);
	vector<pair<float, float>>nearList, newList;
	vector<pair<int, int>>path;
	//RRT核心函數
	bool flag = rrt.RRTCore(nearList, newList,numer);
	if (flag == true)
	{
		//通過RRT獲得路徑
		rrt.CreatePath(path);
		std::cout << "path size is:" << path.size() << std::endl;
		//顯示函數
		Display(xStart, yStart, xGoal, yGoal, mapData, path, nearList, newList);
	}
	return 0;
}

3.2 地圖數據的獲取

本次地圖數據通過python程序將地圖圖片中的障礙物的位置存儲起來,然後通過C++流的方式進行讀取。

img2txt.py:該程序可以將彩蛇或者黑白地圖中的障礙物**(gray_img [ i ] [ j ] [i][j] [i][j]== 0,數據0在圖片中為純黑,表示障礙物;255在圖片中為純白,表示自由可通行區域)**讀取,然後以txt的格式進行存儲。python程序需要opencv的環境,大傢自己百度安裝。

# -*- coding: utf-8 -*-
import matplotlib.pyplot as plt # plt 用於顯示圖片
import numpy as np
import cv2
img = cv2.imread("map/map6.bmp")
print(img.shape)
if len(img.shape)==3:
    print("圖片為彩色圖") 
    gray_img = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
elif len(img.shape)==2:
    print("圖片為灰度圖")
    gray_img=img
h=gray_img.shape[0]
w=gray_img.shape[1]
print (gray_img.shape)
f = open("map/map6.txt", "wb")
# 尺寸 h, w
f.write((str(h) + " " + str(w) + "
").encode("utf-8"))
for i in range(h):
    for j in range(w):
        if gray_img[i][j] == 0:
            print("hello  world")
            f.write((str(i) + " " + str(j) + "
").encode("utf-8"))
            
f.close()
print ("map.txt save sucessful")

其中保存的地圖txt數據分為兩部分,第一行表示地圖的高和寬;從第二行開始表示障礙物的坐標值,例如234 648表示第648行,第234列那個點的圖像像素值為0。圖像坐標系中原點坐標在圖像的左上角,x軸向右,y軸向下。

800 800
234 648
234 649
234 650
234 651
234 652
234 653
234 654
234 655
234 656
234 657
234 658
234 659

map.h

#pragma once
#ifndef __MAP__
#define __MAP__
#include <vector>
#include<iostream>
#include <string>
using namespace std;
vector<vector<int>> MapData(std::string _MapPath);

#endif

map.cpp:通過C++流的方式進行txt數據的讀取,按照上述存儲方式進行讀取。

/*該函數是讀取map.txt形成一個二維數組num,其中num裡面0表示障礙物,255為可自由區域*/
#include "map.h"
#include<fstream>
#include<sstream>
vector<vector<int>>MapData(std::string _MapPath)
{
	ifstream f;
	f.open(_MapPath);
	string str;
	vector<vector<int> > num;
	bool  FirstLine = true;
	while (getline(f, str))      //讀取1行並將它賦值給字符串str
	{
		if (FirstLine)
		{
			istringstream in(str);   // c++風格的串流的輸入操作
			int a;
			in >> a;
			int height = a;
		    in >> a;
			int wight = a;
			num.resize(height, vector<int>(wight, 255));
			FirstLine = false;
		}
		else
		{
			istringstream input(str);   // c++風格的串流的輸入操作
			vector<int> tmp;
			int a;
			while (input >> a)          //通過input將第一行的數據一個一個的輸入給a
				tmp.push_back(a);
			num[tmp[0]][tmp[1]] = 0;
		}
	}
	return num;
}

3.3 RRT算法的實現

RRT.h:主要通過RRTCore函數實現RRT算法的本體,通過CreatePath函數獲得RRT算法的路徑。

#pragma once
#ifndef __RRT__
#define __RRT__
#include <iostream>
#include <string>
#include <vector>
#include <math.h>
#include "ctime"
#define N 999
#define pi 3.1415926

using namespace std;
//定義RRT搜索樹結構
struct Tree
{
	float Sx;          //當前點的x值
	float Sy;          //當前點的y值           //new點
	float SxPrev;      //該點先前點的x值
	float SyPrev;      //該點先前點的x值       //near點
	float Sdist;       //near點與new點的距離
	int SindPrev;      //new點的索引
};
class CRRT
{
public:
	//RRT構造函數
	CRRT(int _xStart, int _yStart, int _xGoal, int _yGoal, int _thr, int _delta, vector<vector<int>>_map);
	//距離計算函數
	inline float GetDist(int _x1, int _y1, int _x2, int _y2);
	//與rand點距離較近的點在RRT樹中的索引
	int GetMinIndex(int _x1, int _y1, vector<Tree>_T);
	//點的安全性判定
	inline bool FeasiblePoint(float _x, float _y, vector<vector<int>>_map);
	//near點與new點連線之間的碰撞檢測
	bool CollisionChecking(vector<float> _startPose, vector<float> _goalPose, vector<vector<int>>_map);
	//RRT核心函數
	bool RRTCore(int _numer,vector<pair<float,float>>& _nearList, vector<pair<float, float>>& _newList);
	//RRT生成路徑函數
	void CreatePath(vector<pair<int, int>>& _path);
private:
	vector<Tree> m_TreeList;       //RRT搜索樹列表
	Tree m_tree;                   //RRT搜索樹
	vector<vector<int>>m_map;      //二維地圖

	int m_xStart;                  //起點X值
	int m_yStart;                  //起點Y值

	int m_xGoal;                   //終點X值
	int m_yGoal;                   //終點Y值

	int m_thr;                     //距離閾值
	int m_delta;                   //步長
	int m_mapWight;                //地圖寬度
	int m_mapHight;                //地圖高度
};


#endif

RRT.cpp:主要實現RRT.h頭文件中的各成員函數。

#include "RRT.h"


/***********************************************************
*@函數功能:       RRT構造函數,對地圖寬度和高度進行初始化
-----------------------------------------------------------
*@函數參數:       _xStart  起點X值
				  _yStart  起點Y值
				  _xGoal   終點X值
	              _yGoal   終點Y值
	              _thr     距離閾值
	              _delta   步長
	              _map     地圖
-----------------------------------------------------------
*@函數返回:      無
***********************************************************/
CRRT::CRRT(int _xStart, 
	      int _yStart, 
	      int _xGoal, 
	      int _yGoal, 
	      int _thr, 
	      int _delta, 
	      vector<vector<int>>_map
):m_xStart(_xStart),m_yStart(_yStart),m_xGoal(_xGoal),m_yGoal(_yGoal),m_thr(_thr),m_delta(_delta),m_map(_map)
{
	m_mapWight = _map[0].size();
	m_mapHight = _map.size();
}

/***********************************************************
*@函數功能:       兩點距離計算函數
-----------------------------------------------------------
*@函數參數:       _x1  第一個點X值
				  _y1  第一個點Y值
				  _x2  第二個點X值
				  _y2  第二個點Y值
-----------------------------------------------------------
*@函數返回:      兩點之間的距離值
***********************************************************/
inline float CRRT::GetDist(int _x1, int _y1, int _x2, int _y2)
{
	return sqrt(pow((_x1 - _x2), 2) + pow((_y1 - _y2), 2));
}

/***********************************************************
*@函數功能:      求rand點距離較近的點在RRT樹中的索引
-----------------------------------------------------------
*@函數參數:       _x1  rand點X值
				  _y1  rand點Y值
				  _T   RRT搜索樹列表
-----------------------------------------------------------
*@函數返回:      最近索引值
***********************************************************/
int CRRT::GetMinIndex(int _x1, int _y1, vector<Tree>_T)
{
	float distance = FLT_MAX;            //FLT_MAX表示float最大值
	int index = 0;
	for (int i = 0; i < _T.size(); i++)
	{
		if (GetDist(_x1, _y1, _T[i].Sx, _T[i].Sy) < distance)
		{
			distance = GetDist(_x1, _y1, _T[i].Sx, _T[i].Sy);
			index = i;
		}
	}
	return index;
}

/***********************************************************
*@函數功能:      點的安全性判定
-----------------------------------------------------------
*@函數參數:       _x1  點X值
				  _y1  點Y值
                  _map 地圖點
-----------------------------------------------------------
*@函數返回:      true表示該點安全,false表示不安全
***********************************************************/
inline bool CRRT::FeasiblePoint(float _x, float _y, vector<vector<int>>_map)
{
	//判斷該點是否在地圖的高度和寬度范圍內,且是否在障礙物內
	if (!(_x >= 0 && _x < m_mapWight && _y >= 0 && _y < m_mapHight && _map[_y][_x] == 255))
		return false;
	return true;
}

/***********************************************************
*@函數功能:      near點與new點連線之間的碰撞檢測
-----------------------------------------------------------
*@函數參數:       _startPose  near點
				  _goalPose   new點
                  _map 地圖點
-----------------------------------------------------------
*@函數返回:      true表示安全,false表示不安全
***********************************************************/
bool CRRT::CollisionChecking(vector<float> _startPose, vector<float> _goalPose, vector<vector<int>>_map)
{
	//new點若在障礙物內,直接返回false
	if (!(FeasiblePoint(floor(_goalPose[0]), ceil(_goalPose[1]), _map)))
	{
		return false;
	}
	float dir = atan2(_goalPose[0] - _startPose[0], _goalPose[1] - _startPose[1]);
	float poseCheck[2] = { 0.0,0.0 };
	float stop = sqrt(pow(_startPose[0] - _goalPose[0], 2) + pow(_startPose[1] - _goalPose[1], 2));
	//r+=2表示在near與new連線的基礎上,每次移動2個步長
	for (float r = 0; r <= stop; r += 2)
	{
		poseCheck[0] = _startPose[0] + r*sin(dir);
		poseCheck[1] = _startPose[1] + r*cos(dir);
		//由於poseCheck點為float類型,為亞像素級,因此判斷該點四領域的像素值,ceil向上取整,floor向下取整
		if (!(FeasiblePoint(ceil(poseCheck[0]), ceil(poseCheck[1]), _map) && 
		FeasiblePoint(floor(poseCheck[0]), floor(poseCheck[1]), _map) && 
		FeasiblePoint(ceil(poseCheck[0]), floor(poseCheck[1]), _map) && 
		FeasiblePoint(floor(poseCheck[0]), ceil(poseCheck[1]), _map)))
		{
			return false;
		}
	}
	return true;
}


/***********************************************************
*@函數功能:   RRT核心函數
-----------------------------------------------------------
*@函數參數:   _nearList  near點集合,為引用傳參,實際上也為返回值
			  _newList    new點集合,為引用傳參,實際上也為返回值
			  _numer     RRT算法迭代次數
-----------------------------------------------------------
*@函數返回:   true表示RRT找到路徑,false表示沒找到
***********************************************************/
bool CRRT::RRTCore(int _numer,vector<pair<float, float>>& _nearList, vector<pair<float, float>>& _newList)
{
	//將起點插入樹中
	m_tree.Sx =m_xStart;
	m_tree.Sy = m_yStart;
	m_tree.SxPrev = m_xGoal;
	m_tree.SyPrev = m_yGoal;
	m_tree.Sdist = 0;
	m_tree.SindPrev = 0;
	m_TreeList.push_back(m_tree);

	vector<float>Rand, Near, New;
	Rand.resize(2, 0.0);
	Near.resize(2, 0.0);
	New.resize(2, 0.0);

	srand(time(NULL));//設置隨機數種子,使每次產生的隨機序列不同
	int count = 0;
	for (int i = 1; i <= _numer; i++)
	{
		//隨機產生地圖點Rand
		Rand[0] =m_mapWight*(rand() % (N + 1) / (float)(N + 1));
		Rand[1] = m_mapHight*(rand() % (N + 1) / (float)(N + 1));

		//通過距離判斷來計算與Rand最近的Near點
		int minDisIndex = GetMinIndex(Rand[0], Rand[1], m_TreeList);
		Near[0] = m_TreeList[minDisIndex].Sx;
		Near[1] = m_TreeList[minDisIndex].Sy;

		//Near與Rand連線,移動delta步長
		float theta = atan2(Rand[1] - Near[1], Rand[0] - Near[0]);
		New[0] = Near[0] + m_delta*(cos(theta));
		New[1] = Near[1] + m_delta*(sin(theta));

		//連線碰撞檢測
		if (!CollisionChecking(Near, New, m_map))
			continue;

		//擴展RRT搜索樹
		std::cout << "i:" << i << std::endl;
		m_tree.Sx = New[0];
		m_tree.Sy = New[1];
		m_tree.SxPrev = Near[0];
		m_tree.SyPrev = Near[1];
		m_tree.Sdist = m_delta;
		m_tree.SindPrev = minDisIndex;
		m_TreeList.emplace_back(m_tree);

		//距離閾值判斷,是否搜索結束
		if (GetDist(New[0], New[1], m_xGoal, m_yGoal) < m_thr)
		{
			return true;
		}

		//保存near點與new點
		_nearList.emplace_back(std::make_pair(Near[0], Near[1]));
		_newList.emplace_back(std::make_pair(New[0], New[1]));
	}
	return false;

}

/***********************************************************
*@函數功能:   RRT生成路徑,逆向搜索
-----------------------------------------------------------
*@函數參數:   _path  RRT生成路徑集合點,為引用傳參,實際上也為返回值
-----------------------------------------------------------
*@函數返回:   無
***********************************************************/
void CRRT::CreatePath(vector<pair<int, int>>& _path)
{
	pair<int, int>temp;
	//將終點加入路徑集合點
	_path.push_back(std::make_pair(m_xGoal, m_yGoal));
	//由於搜索路徑結束存在一個閾值,故將搜索樹的最後一個點加入路徑集合點
	_path.push_back(std::make_pair(m_TreeList[m_TreeList.size() - 1].Sx, m_TreeList[m_TreeList.size() - 1].Sy));

	int pathIndex = m_TreeList[m_TreeList.size() - 1].SindPrev;
	//逆向搜索
	while (true)
	{
		_path.emplace_back(std::make_pair(m_TreeList[pathIndex].Sx, m_TreeList[pathIndex].Sy));

		pathIndex = m_TreeList[pathIndex].SindPrev;
		if (pathIndex == 0)
			break;
	}
	//將起點加入路徑集合點
	_path.push_back(std::make_pair(m_TreeList[0].Sx, m_TreeList[0].Sy));
	
}

接下裡主要從RRT中的核心函數RRTCore進行算法介紹。

3.3.1 起點入樹

#include "RRT.h"
/***********************************************************
*@函數功能:       RRT構造函數,對地圖寬度和高度進行初始化
-----------------------------------------------------------
*@函數參數:       _xStart  起點X值
				  _yStart  起點Y值
				  _xGoal   終點X值
	              _yGoal   終點Y值
	              _thr     距離閾值
	              _delta   步長
	              _map     地圖
-----------------------------------------------------------
*@函數返回:      無
***********************************************************/
CRRT::CRRT(int _xStart, 
	      int _yStart, 
	      int _xGoal, 
	      int _yGoal, 
	      int _thr, 
	      int _delta, 
	      vector<vector<int>>_map
):m_xStart(_xStart),m_yStart(_yStart),m_xGoal(_xGoal),m_yGoal(_yGoal),m_thr(_thr),m_delta(_delta),m_map(_map)
{
	m_mapWight = _map[0].size();
	m_mapHight = _map.size();
}
/***********************************************************
*@函數功能:       兩點距離計算函數
-----------------------------------------------------------
*@函數參數:       _x1  第一個點X值
				  _y1  第一個點Y值
				  _x2  第二個點X值
				  _y2  第二個點Y值
-----------------------------------------------------------
*@函數返回:      兩點之間的距離值
***********************************************************/
inline float CRRT::GetDist(int _x1, int _y1, int _x2, int _y2)
{
	return sqrt(pow((_x1 - _x2), 2) + pow((_y1 - _y2), 2));
}
/***********************************************************
*@函數功能:      求rand點距離較近的點在RRT樹中的索引
-----------------------------------------------------------
*@函數參數:       _x1  rand點X值
				  _y1  rand點Y值
				  _T   RRT搜索樹列表
-----------------------------------------------------------
*@函數返回:      最近索引值
***********************************************************/
int CRRT::GetMinIndex(int _x1, int _y1, vector<Tree>_T)
{
	float distance = FLT_MAX;            //FLT_MAX表示float最大值
	int index = 0;
	for (int i = 0; i < _T.size(); i++)
	{
		if (GetDist(_x1, _y1, _T[i].Sx, _T[i].Sy) < distance)
		{
			distance = GetDist(_x1, _y1, _T[i].Sx, _T[i].Sy);
			index = i;
		}
	}
	return index;
}
/***********************************************************
*@函數功能:      點的安全性判定
-----------------------------------------------------------
*@函數參數:       _x1  點X值
				  _y1  點Y值
                  _map 地圖點
-----------------------------------------------------------
*@函數返回:      true表示該點安全,false表示不安全
***********************************************************/
inline bool CRRT::FeasiblePoint(float _x, float _y, vector<vector<int>>_map)
{
	//判斷該點是否在地圖的高度和寬度范圍內,且是否在障礙物內
	if (!(_x >= 0 && _x < m_mapWight && _y >= 0 && _y < m_mapHight && _map[_y][_x] == 255))
		return false;
	return true;
}
/***********************************************************
*@函數功能:      near點與new點連線之間的碰撞檢測
-----------------------------------------------------------
*@函數參數:       _startPose  near點
				  _goalPose   new點
                  _map 地圖點
-----------------------------------------------------------
*@函數返回:      true表示安全,false表示不安全
***********************************************************/
bool CRRT::CollisionChecking(vector<float> _startPose, vector<float> _goalPose, vector<vector<int>>_map)
{
	//new點若在障礙物內,直接返回false
	if (!(FeasiblePoint(floor(_goalPose[0]), ceil(_goalPose[1]), _map)))
	{
		return false;
	}
	float dir = atan2(_goalPose[0] - _startPose[0], _goalPose[1] - _startPose[1]);
	float poseCheck[2] = { 0.0,0.0 };
	float stop = sqrt(pow(_startPose[0] - _goalPose[0], 2) + pow(_startPose[1] - _goalPose[1], 2));
	//r+=2表示在near與new連線的基礎上,每次移動2個步長
	for (float r = 0; r <= stop; r += 2)
	{
		poseCheck[0] = _startPose[0] + r*sin(dir);
		poseCheck[1] = _startPose[1] + r*cos(dir);
		//由於poseCheck點為float類型,為亞像素級,因此判斷該點四領域的像素值,ceil向上取整,floor向下取整
		if (!(FeasiblePoint(ceil(poseCheck[0]), ceil(poseCheck[1]), _map) && 
		FeasiblePoint(floor(poseCheck[0]), floor(poseCheck[1]), _map) && 
		FeasiblePoint(ceil(poseCheck[0]), floor(poseCheck[1]), _map) && 
		FeasiblePoint(floor(poseCheck[0]), ceil(poseCheck[1]), _map)))
		{
			return false;
		}
	}
	return true;
}
/***********************************************************
*@函數功能:   RRT核心函數
-----------------------------------------------------------
*@函數參數:   _nearList  near點集合,為引用傳參,實際上也為返回值
			  _newList    new點集合,為引用傳參,實際上也為返回值
			  _numer     RRT算法迭代次數
-----------------------------------------------------------
*@函數返回:   true表示RRT找到路徑,false表示沒找到
***********************************************************/
bool CRRT::RRTCore(int _numer,vector<pair<float, float>>& _nearList, vector<pair<float, float>>& _newList)
{
	//將起點插入樹中
	m_tree.Sx =m_xStart;
	m_tree.Sy = m_yStart;
	m_tree.SxPrev = m_xGoal;
	m_tree.SyPrev = m_yGoal;
	m_tree.Sdist = 0;
	m_tree.SindPrev = 0;
	m_TreeList.push_back(m_tree);
	vector<float>Rand, Near, New;
	Rand.resize(2, 0.0);
	Near.resize(2, 0.0);
	New.resize(2, 0.0);
	srand(time(NULL));//設置隨機數種子,使每次產生的隨機序列不同
	int count = 0;
	for (int i = 1; i <= _numer; i++)
	{
		//隨機產生地圖點Rand
		Rand[0] =m_mapWight*(rand() % (N + 1) / (float)(N + 1));
		Rand[1] = m_mapHight*(rand() % (N + 1) / (float)(N + 1));
		//通過距離判斷來計算與Rand最近的Near點
		int minDisIndex = GetMinIndex(Rand[0], Rand[1], m_TreeList);
		Near[0] = m_TreeList[minDisIndex].Sx;
		Near[1] = m_TreeList[minDisIndex].Sy;
		//Near與Rand連線,移動delta步長
		float theta = atan2(Rand[1] - Near[1], Rand[0] - Near[0]);
		New[0] = Near[0] + m_delta*(cos(theta));
		New[1] = Near[1] + m_delta*(sin(theta));
		//連線碰撞檢測
		if (!CollisionChecking(Near, New, m_map))
			continue;
		//擴展RRT搜索樹
		std::cout << "i:" << i << std::endl;
		m_tree.Sx = New[0];
		m_tree.Sy = New[1];
		m_tree.SxPrev = Near[0];
		m_tree.SyPrev = Near[1];
		m_tree.Sdist = m_delta;
		m_tree.SindPrev = minDisIndex;
		m_TreeList.emplace_back(m_tree);
		//距離閾值判斷,是否搜索結束
		if (GetDist(New[0], New[1], m_xGoal, m_yGoal) < m_thr)
		{
			return true;
		}
		//保存near點與new點
		_nearList.emplace_back(std::make_pair(Near[0], Near[1]));
		_newList.emplace_back(std::make_pair(New[0], New[1]));
	}
	return false;
}
/***********************************************************
*@函數功能:   RRT生成路徑,逆向搜索
-----------------------------------------------------------
*@函數參數:   _path  RRT生成路徑集合點,為引用傳參,實際上也為返回值
-----------------------------------------------------------
*@函數返回:   無
***********************************************************/
void CRRT::CreatePath(vector<pair<int, int>>& _path)
{
	pair<int, int>temp;
	//將終點加入路徑集合點
	_path.push_back(std::make_pair(m_xGoal, m_yGoal));
	//由於搜索路徑結束存在一個閾值,故將搜索樹的最後一個點加入路徑集合點
	_path.push_back(std::make_pair(m_TreeList[m_TreeList.size() - 1].Sx, m_TreeList[m_TreeList.size() - 1].Sy));
	int pathIndex = m_TreeList[m_TreeList.size() - 1].SindPrev;
	//逆向搜索
	while (true)
	{
		_path.emplace_back(std::make_pair(m_TreeList[pathIndex].Sx, m_TreeList[pathIndex].Sy));
		pathIndex = m_TreeList[pathIndex].SindPrev;
		if (pathIndex == 0)
			break;
	}
	//將起點加入路徑集合點
	_path.push_back(std::make_pair(m_TreeList[0].Sx, m_TreeList[0].Sy));
	
}

3.3.2 rand點的獲取

為瞭方便起見,並沒有設置隨機采樣函數,通過隨機種子進行rand的確定。其中(rand() % (N + 1) / (float)(N + 1))是產生0~1的隨機樹,小數點與N有關。

m_tree.Sx =m_xStart;
m_tree.Sy = m_yStart;
m_tree.SxPrev = m_xGoal;
m_tree.SyPrev = m_yGoal;
m_tree.Sdist = 0;
m_tree.SindPrev = 0;
m_TreeList.push_back(m_tree);
vector<float>Rand, Near, New;
Rand.resize(2, 0.0);
Near.resize(2, 0.0);
New.resize(2, 0.0);

3.3.3 near點的獲取

通過簡單的距離函數進行near點的判斷。其中GetMinIndex就是通過遍歷獲取與rand點最近的near點,當然可以通過kd-tree對這塊進行改進,大傢感興趣可以自行發揮,這裡為瞭方便起見,就采用最原始的方法。

//隨機產生地圖點Rand
Rand[0] =m_mapWight*(rand() % (N + 1) / (float)(N + 1));
Rand[1] = m_mapHight*(rand() % (N + 1) / (float)(N + 1));

3.3.4 new點的獲取

//通過距離判斷來計算與Rand最近的Near點
int minDisIndex = GetMinIndex(Rand[0], Rand[1], m_TreeList);
Near[0] = m_TreeList[minDisIndex].Sx;
Near[1] = m_TreeList[minDisIndex].Sy;

註意near點的獲取使用C++中的atan2函數,該函數是 atan() 的增強版,能夠確定角度所在的象限。

其中**double atan2(double y,double x)**返回 y/x 的反正切值,以弧度表示,取值范圍為(-π,π]。如下圖所示,紅色線為 s i n ( θ ) sin( heta) sin(θ),綠色線為 c o s ( θ ) cos( heta) cos(θ)。

當 (x, y) 在象限中時:

第一象限

第二象限

第三象限

第四象限

0 < θ < π / 2 0< heta<pi/2 0<θ<π/2

π / 2 < θ < π pi/2 < heta <pi π/2<θ<π

π < θ < π / 2 -pi< heta<-pi/2 π<θ<π/2

π / 2 < θ < 0 -pi/2< heta<0 π/2<θ<0

當 (x, y) 在象限的邊界(也就是坐標軸)上時:

y = 0 y=0 y=0且 x ≥ 0 x geq 0 x≥0

y = 0 y=0 y=0且 x < 0 x < 0 x<0

y > 0 y>0 y>0且 x = 0 x=0 x=0

y < 0 y<0 y<0且 x = 0 x=0 x=0

θ = 0 heta =0 θ=0

θ = π heta=pi θ=π

θ = π / 2 heta=pi/2 θ=π/2

θ = π / 2 heta=-pi/2 θ=π/2

那麼
n e w _ x = n e a r _ x + d c o s ( θ ) n e w _ y = n e a e _ y + d s i n ( θ ) new_x=near_x+d*cos( heta) \ new_y=neae_y+d*sin( heta) \ new_x=near_x+dcos(θ)new_y=neae_y+dsin(θ)

表示new點的情況如下,均滿足new點在near與rand點之間。這就是atan2帶來的好處。

第一象限

第二象限

第三象限

第四象限

3.3.5 安全性檢測

near點與new點之間的安全性判斷通過CollisionChecking函數所實習,基本思想就是沿著near與new點的方向,每隔一定的微小步長(代碼中用 r r r)取一點,判斷該點是否在障礙物內。註意微小步長所取的點,它的像素是亞像素級的,可通過雙線性插值方法判斷該像素的值。本文為瞭方便起見,判斷該點亞像素的周圍四點領域,進行安全性判斷。

舉個例子,例如該點為 p = ( 1.3 , 4.7 ) p=(1.3,4.7) p=(1.3,4.7),通過向下取整floor和向上取整ceil得該亞像素點的四點領域

c e i l ( 1.3 ) = 2 , c e i l ( 4.7 ) = 5 > p 1 = ( 2 , 5 ) ceil(1.3)=2,ceil(4.7) =5 —>p_1=(2,5) ceil(1.3)=2,ceil(4.7)=5>p1=(2,5)

f l o o r ( 1.3 ) = 1 , f l o o r ( 4.7 ) = 4 > p 2 = ( 1 , 4 ) floor(1.3)=1,floor(4.7)=4–>p_2=(1,4) floor(1.3)=1,floor(4.7)=4>p2=(1,4)

c e i l ( 1.3 ) = 2 , f l o o r ( 4.7 ) = 4 > p 3 = ( 2 , 4 ) ceil(1.3)=2,floor(4.7)=4–>p_3=(2,4) ceil(1.3)=2,floor(4.7)=4>p3=(2,4)

f l o o r ( 1.3 ) = 1 , c e i l ( 4.7 ) = 5 > p 4 = ( 1 , 5 ) floor(1.3)=1,ceil(4.7) =5—>p_4=(1,5) floor(1.3)=1,ceil(4.7)=5>p4=(1,5)

bool CRRT::CollisionChecking(vector<float> _startPose, vector<float> _goalPose, vector<vector<int>>_map)
{
	//new點若在障礙物內,直接返回false
	if (!(FeasiblePoint(floor(_goalPose[0]), ceil(_goalPose[1]), _map)))
	{
		return false;
	}
	float dir = atan2(_goalPose[0] - _startPose[0], _goalPose[1] - _startPose[1]);
	float poseCheck[2] = { 0.0,0.0 };
	float stop = sqrt(pow(_startPose[0] - _goalPose[0], 2) + pow(_startPose[1] - _goalPose[1], 2));
	//r+=2表示在near與new連線的基礎上,每次移動2個步長
	for (float r = 0; r <= stop; r += 2)
	{
		poseCheck[0] = _startPose[0] + r*sin(dir);
		poseCheck[1] = _startPose[1] + r*cos(dir);
		//由於poseCheck點為float類型,為亞像素級,因此判斷該點四領域的像素值,ceil向上取整,floor向下取整
		if (!(FeasiblePoint(ceil(poseCheck[0]), ceil(poseCheck[1]), _map) && 
		FeasiblePoint(floor(poseCheck[0]), floor(poseCheck[1]), _map) && 
		FeasiblePoint(ceil(poseCheck[0]), floor(poseCheck[1]), _map) && 
		FeasiblePoint(floor(poseCheck[0]), ceil(poseCheck[1]), _map)))
		{
			return false;
		}
	}
	return true;
}

3.4 可視化顯示

display.h

#pragma once
#ifndef __DISPLAY__
#define __DISPLAY__
#include <opencv2/opencv.hpp>
#include <vector>
using namespace std;
//顯示函數
void Display(int _xStart,int _yStart,int _xGoal,int _yGoal,
	         vector<vector<int>>_map, 
	         vector<pair<int, int>>_path, 
	         vector<pair<float, float>>_nearList, 
	         vector<pair<float, float>>_newList);
#endif // !__DISPLAY__

display.cpp

註意該代碼會在當前項目中的image文件夾(沒有該文件夾,手動創建一個即可)中存儲rrt顯示過程圖片(為瞭後期作gif使用,其他沒什麼用),若是不想存儲,則註釋掉。

cv::imwrite(“image/image” + std::to_string(i) + “.jpg”, image);

#include "display.h"
#include <iostream>
#include <string>
#include <Windows.h>
#include <cstdlib>
#include <io.h>      // _access
#include <direct.h>  //_mkdir
/***********************************************************
*@函數功能:       RRT函數顯示
-----------------------------------------------------------
*@函數參數:       _xStart  起點X值
				  _yStart  起點Y值
				  _xGoal   終點X值
                  _yGoal   終點Y值
                  _thr     距離閾值
                  _map     地圖
				  _path    路徑點
				  _nearList near點集合
				  _newList  new點集合
-----------------------------------------------------------
*@函數返回:      無
***********************************************************/
void Display(int _xStart,
	         int _yStart, 
	         int _xGoal, 
	         int _yGoal,
	         vector<vector<int>>_map, 
	         vector<pair<int, int>>_path, 
	         vector<pair<float, float>>_nearList, 
	         vector<pair<float, float>>_newList)
{
	int mapWight = _map[0].size();
	int mapHight = _map.size();

	//如沒有image文件夾,則新建一個,存放RRT擴展樹的中間過程
	std::string prefix = "image";
	if (_access(prefix.c_str(), 0) == -1)	//如果文件夾不存在
	{
		_mkdir(prefix.c_str());    //則創建
	}

	//通過地圖點繪制圖像RGB,白色可通行區域,黑色為障礙物區域
	cv::namedWindow("RRT result", CV_WINDOW_AUTOSIZE);
	cv::Mat image(mapHight, mapWight, CV_8UC3, cv::Scalar(0, 0, 0));
	for (int row = 0; row < mapHight; row++)
	{
		for (int col = 0; col < mapWight; col++)
		{
			if (_map[row][col] == 255)
			{
				image.at<cv::Vec3b>(row, col)[0] = 255;
				image.at<cv::Vec3b>(row, col)[1] = 255;
				image.at<cv::Vec3b>(row, col)[2] = 255;
			}
		}
	}
	//顯示起點和終點,紅色起點,綠色終點
	cv::circle(image, cv::Point(_xStart, _yStart), 4, cv::Scalar(0, 0, 255), -1, 4);   //起點
	cv::circle(image, cv::Point(_xGoal, _yGoal), 4, cv::Scalar(0, 255, 0), -1, 4);    //終點

	//顯示路徑探索過程
	for (int i = 0; i < _nearList.size(); i++)
	{
		cv::line(image, cv::Point(_nearList[i].first, _nearList[i].second), cv::Point(_newList[i].first, _newList[i].second), cv::Scalar(255, 0, 0), 2, 2);
		cv::imshow("RRT result", image);
		cv::waitKey(100);  //100ms刷新一下
		cv::imwrite("image/image" + std::to_string(i) + ".jpg", image);
	}
	//顯示最終路徑,黃色
	for (int i = 0; i < _path.size() - 1; i++)
	{
		cv::line(image, cv::Point(_path[i].first, _path[i].second), cv::Point(_path[i + 1].first, _path[i + 1].second), cv::Scalar(0, 255, 255), 2, 2);
	}
	//保存6張最終圖片,方便制作gif
	for (int i = 0; i <= 5; i++)
	{
	    cv::imwrite("image/image"+std::to_string(_nearList.size()+i)+".jpg", image);
	}
	cv::imshow("RRT result", image);
	cv::waitKey(0);
}

4. 代碼運行過程

註意顯示過程中的“樹枝”表示near點與new點的連接。

顯示過程

顯示結果

map6.bmp

顯示過程

顯示結果

map.png

<動圖太大,CSDN僅支持5M,無法顯示>

一個批量將圖片轉為gif的python腳本,註意python代碼中一定要添加dir_list = natsort.natsorted(dir_list),否則會出現圖片亂序的問題。

import os
import cv2 as cv
import moviepy.editor as mpy
import numpy as np
import natsort
import imageio
def frame_to_gif(frame_list):
    gif = imageio.mimsave('./result.gif', frame_list, 'GIF', duration=0.00085)  
dir_list = os.listdir('image')
dir_list = natsort.natsorted(dir_list)
img_list=[]
for i in range(0,len(dir_list)):
    print (dir_list[i])
    img = cv.imread('image\' + dir_list[i])
    #img = cv.cvtcolor(img, cv.color_bgr2rgb)
    img_list.append(img)
frame_to_gif(img_list)

到此這篇關於C++結合OpenCV實現RRT算法的文章就介紹到這瞭,更多相關C++ OpenCV RRT算法內容請搜索LevelAH以前的文章或繼續瀏覽下面的相關文章希望大傢以後多多支持LevelAH!

推薦閱讀: