RRT路径搜索算法C++实现

基于RRT的路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径。适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。

对图片进行灰度图处理，并对像素点进行修改为0和255两个值，来代替二维栅格地图，来实现rrt路径搜索算法，c++主要类实现如下图：

class RRT_Eigen
{
public:
	RRT_Eigen(MatrixXd startpoint, MatrixXd endpoint);
	~RRT_Eigen();
	void rrt(Mat map);
	MatrixXd find_qNew(MatrixXd a, MatrixXd b);
	bool equal_vers(MatrixXd a, MatrixXd b);
	bool goalonedges(MatrixXd a, MatrixXd b,int c);
	bool Edge_freespace(Mat map, MatrixXd a, MatrixXd b);
	MatrixXd matrix_together(MatrixXd a, MatrixXd b);
	vector<int> fillsolutionPath(MatrixXd p);
	void path_smooth(Mat map);
	void rrtDraw(Mat img);
	bool ISEdgebelongfree(Mat map, MatrixXd a, MatrixXd b);
private:
	vector<int> Path, smooth;//path存rrt找到的路径 smooth存优化过的路径
	MatrixXd Start, End;//起点和终点
	//k=迭代次数  delta_q结点之间的距离  p随机值概率  
	//delta判断边是否在障碍物上的参数
	double k = 1000, delta_q = 50, p = 0.3, delta = 3;
	MatrixXd vertices, edges;//vertices存结点 edges 路径回溯矩阵
	MatrixXd q_near, q_rand, q_new;
	//q_near 离随机值近的结点  q_rand 随机点 q_new新结点
	int q_nearIndex;//存到edges矩阵中帮助回溯路径
};

传入图片为：