(二)路径规划算法---C++结合OpenCV实现RRT算法

C++结合OpenCV实现RRT算法

文章目录

• C++结合OpenCV实现RRT算法
• • 1.RRT算法简介
  • 2.算法整体框架流程
  • • 2.1 rand点的建立
    • 2.2 near和new点的建立
    • 2.3 安全性检查
    • 2.4 算法结束判断
  • 3.RRT代码框架
  • • 3.1 主函数
    • 3.2 地图数据的获取
    • 3.3 RRT算法的实现
    • • 3.3.1 起点入树
      • 3.3.2 rand点的获取
      • 3.3.3 near点的获取
      • 3.3.4 new点的获取
      • 3.3.5 安全性检测
    • 3.4 可视化显示
  • 4. 代码运行过程

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算法的接口函数RRTCore和CreatePath获得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