机器人C++库(12) Robotics Library 之路径规划算法:PRM、RRT、EET算法

news2025/1/16 9:02:47

机器人C++库(12)Robotics Library 之路径规划算法:PRM、RRT、EET算法

RL库的运动规划(rl::plan)模块集成了以下经典的路径规划算法:

  • PRM算法:概率路线图算法
  • RRT算法:快速探索随机树算法
  • EET算法:搜索树算法-基于采样:https://blog.csdn.net/yohnyang/article/details/127783244

另外,补充一个开源运动规划库OMPL:https://ompl.kavrakilab.org/index.html

下边参考官方给出来的例程讲述路径规划算法原理及使用,规划效果如下图所示:

在这里插入图片描述

左图是PRM算法进行路径规划的结果,灰色路径是拟生成所有路径,绿色路径是最优路径;右图是采用EET搜索树算法进行规划的结果,绿色球球心连接起点与终点的最短路径即为最优路径。

1.PRM算法

概率路线图(Probabilistic Roadmap,PRM)属于综合查询方法,其步骤如下:

  • Step1:预处理
    在这里插入图片描述
  • Step2:搜索
    采用图搜索算法对无向图G进行搜索(A*搜索),如果能找到起始点A到终点B的路线,说明存在可行的运动规划方案。
    在这里插入图片描述
  • RL库实现:

#include <iostream>
#include <memory>
#include <stdexcept>
#include <boost/lexical_cast.hpp>
#include <rl/mdl/Kinematic.h>
#include <rl/math/Unit.h>
#include <rl/plan/KdtreeNearestNeighbors.h>
#include <rl/plan/Prm.h>  //1
#include<rl/plan/Rrt.h>
#include <rl/plan/RecursiveVerifier.h>
#include <rl/plan/SimpleModel.h>
#include <rl/plan/SimpleOptimizer.h>
#include <rl/plan/UniformSampler.h>
#include <rl/sg/Model.h>

#define RL_SG_BULLET 1

#ifdef RL_SG_BULLET
#include <rl/sg/bullet/Scene.h>
#endif // RL_SG_BULLET
#ifdef RL_SG_FCL
#include <rl/sg/fcl/Scene.h>
#endif // RL_SG_FCL
#ifdef RL_SG_ODE
#include <rl/sg/ode/Scene.h>
#endif // RL_SG_ODE
#ifdef RL_SG_PQP
#include <rl/sg/pqp/Scene.h>
#endif // RL_SG_PQP
#ifdef RL_SG_SOLID
#include <rl/sg/solid/Scene.h>
#endif // RL_SG_SOLID

int main()
{
	std::string scene_path = "./puma560/unimation-puma560_boxes.xml";
	std::string model_path = "./puma560/unimation-puma560.xml";

	//末端位姿
	float X0 = 0;
	float Y0 = 0;
	float Z0 = 0;
	float R = 90;
	float P = 0;
	float Y = 0;

	std::vector<rl::math::Real>angles0{ 90,-180,90,0,0,0 };
	std::vector<rl::math::Real>angles1{ 0,0,90,0,0,0 };

	//定义场景碰撞检测模型
	std::shared_ptr<rl::sg::Scene> scene;
	scene = std::make_shared<rl::sg::bullet::Scene>();


	//读入场景图像
	scene->load(scene_path);
	//读入机械臂模型
	std::shared_ptr<rl::kin::Kinematics> kinematics(rl::kin::Kinematics::create(model_path));

	//word为末端位姿?
	rl::math::Transform world = rl::math::Transform::Identity();

	world = rl::math::AngleAxis(R * ::rl::math::DEG2RAD, ::rl::math::Vector3::UnitZ())
		* ::rl::math::AngleAxis(P * ::rl::math::DEG2RAD, ::rl::math::Vector3::UnitY())
		* ::rl::math::AngleAxis(Y * ::rl::math::DEG2RAD, ::rl::math::Vector3::UnitX());

	world.translation().x() = X0;
	world.translation().y() = Y0;
	world.translation().z() = Z0;

	kinematics->world() = world;

	rl::plan::SimpleModel model;
	model.kin = kinematics.get();
	model.model = scene->getModel(0);
	model.scene = scene.get();

	rl::plan::KdtreeNearestNeighbors nearestNeighbors(&model);
	rl::plan::Prm planner;
	
	rl::plan::UniformSampler sampler;
	rl::plan::RecursiveVerifier verifier;

	sampler.seed(0);

	planner.model = &model;
	planner.setNearestNeighbors(&nearestNeighbors);
	planner.sampler = &sampler;
	planner.verifier = &verifier;

	sampler.model = &model;

	verifier.delta = 1.0f * rl::math::DEG2RAD;
	verifier.model = &model;

	rl::math::Vector start(kinematics->getDof());

	for (std::size_t i = 0; i < kinematics->getDof(); ++i)
	{
		start(i) = boost::lexical_cast<rl::math::Real>(angles0[i]) * rl::math::DEG2RAD;
	}

	planner.start = &start;
	rl::math::Vector goal(kinematics->getDof());

	for (std::size_t i = 0; i < kinematics->getDof(); ++i)
	{
		goal(i) = boost::lexical_cast<rl::math::Real>(angles1[i]) * rl::math::DEG2RAD;
	}

	planner.goal = &goal;
	planner.duration = std::chrono::seconds(20);

	std::cout << "construct() ... " << std::endl;;
	std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now();
	planner.construct(15);
	std::chrono::steady_clock::time_point stopTime = std::chrono::steady_clock::now();
	std::cout << "construct() " << std::chrono::duration_cast<std::chrono::duration<double>>(stopTime - startTime).count() * 1000 << " ms" << std::endl;

	std::cout << "solve() ... " << std::endl;;
	startTime = std::chrono::steady_clock::now();
	bool solved = planner.solve();
	stopTime = std::chrono::steady_clock::now();
	std::cout << "solve() " << (solved ? "true" : "false") << " " << std::chrono::duration_cast<std::chrono::duration<double>>(stopTime - startTime).count() * 1000 << " ms" << std::endl;

	std::cout << "NumVertices: " << planner.getNumVertices() << "  NumEdges: " << planner.getNumEdges() << std::endl;
	
	//读取路径
	rl::plan::VectorList paths = planner.getPath();
	/*rl::plan::VectorList::const_iterator i = paths.begin();
	for (; i != paths.end(); ++i)
	{
		std::cout << *i*rl::math::RAD2DEG << std::endl;
	}*/
	//插值
	rl::plan::VectorList path_;
	rl::plan::VectorList::iterator i = paths.begin();
	rl::plan::VectorList::iterator j = ++paths.begin();
	rl::math::Real length = 0;
	rl::math::Real delta = 0.05;
	for (; i != paths.end() && j != paths.end(); ++i, ++j)
	{
		length += model.distance(*i, *j);
		std::cout << "*i = " << (*i).transpose() * rl::math::RAD2DEG << std::endl << "   , *j = " << (*j).transpose() * rl::math::RAD2DEG << std::endl;

		rl::math::Real steps = std::ceil(model.distance(*i, *j) / delta);
		rl::math::Vector inter(model.getDofPosition());
		for (std::size_t k = 1; k < steps + 1; ++k)
		{
			model.interpolate(*i, *j, k / steps, inter);
			std::cout << "k = " << k << ":  " << inter.transpose() * rl::math::RAD2DEG << std::endl;
			path_.push_back(inter);
		}
	}
	std::cout << "length = " << length << std::endl;

	if (solved)
	{
		/*if (boost::lexical_cast<std::size_t>(argv[4]) >= planner.getNumVertices() &&
			boost::lexical_cast<std::size_t>(argv[5]) >= planner.getNumEdges())*/
		if (17 >= planner.getNumVertices() &&
			16 >= planner.getNumEdges())
		{
			return EXIT_SUCCESS;
		}
		else
		{
			std::cerr << "NumVertices and NumEdges are more than expected for this test case.";
			return EXIT_FAILURE;
		}
	}

	return EXIT_FAILURE;
}

2.RRT算法

RT 算法(快速扩展随机树,rapidly exploring random tree)是一种随机性算法,它可以直接应用于非完整约束系统的规划,不需进行路径转换,所以它的算法复杂度较小,尤为适用于高维多自由度的系统。

  • 缺点是得到的路径质量不是很好。需要后处理进一步优化。
    思想是快速扩张一群像树一样的路径以探索(填充)空间的大部分区域,伺机找到可行的路径。
  • RRT 的基本步骤是:
      1. 起点作为一颗种子,从它开始生长枝丫;
      2. 在机器人的“构型”空间中,生成一个随机点X;
      3. 在树上找到距离X最近的那个点,记为Y吧;
      4. 朝着X的方向生长,如果没有碰到障碍物就把生长后的树枝和端点添加到树上,返回 2;

请添加图片描述

  • 伪代码:
function BuildRRT(qinit, K, Δq)
    T.init(qinit)
    for k = 1 to K
        qrand = Sample()  -- chooses a random configuration
        qnearest = Nearest(T, qrand) -- selects the node in the RRT tree that is closest to qrand
        if  Distance(qnearest, qgoal) < Threshold then
            return true
        qnew = Extend(qnearest, qrand, Δq)  -- moving from qnearest an incremental distance in the direction of qrand
        if qnew ≠ NULL then
            T.AddNode(qnew)
    return false
 
 
function Sample() -- Alternatively,one could replace Sample with SampleFree(by using a collision detection algorithm to reject samples in C_obstacle
    p = Random(0, 1.0)
    if 0 < p < Prob then
        return qgoal
    elseif Prob < p < 1.0 then
        return RandomNode()

3.EET算法

提出一种基于工作空间信息的在探索和开发之间逐步转换的机制,详见:https://blog.csdn.net/yohnyang/article/details/127783244

  • RL库算法实现:这个算法模型比较复杂,参数比较多,调参对算法结果影响很大
#include <iostream>
#include <memory>
#include <stdexcept>
#include <boost/lexical_cast.hpp>
//#include <rl/kin/Kinematics.h>
#include <rl/mdl/Kinematic.h>
#include <rl/math/Unit.h>
#include <rl/plan/DistanceModel.h>
#include <rl/plan/Eet.h>
#include <rl/plan/LinearNearestNeighbors.h>
#include <rl/plan/UniformSampler.h>
#include <rl/plan/WorkspaceSphereExplorer.h>
#include<rl/mdl/XmlFactory.h>
#include <rl/sg/Model.h>

#define RL_SG_BULLET 1

#ifdef RL_SG_BULLET
#include <rl/sg/bullet/Scene.h>
#endif // RL_SG_BULLET
#ifdef RL_SG_FCL
#include <rl/sg/fcl/Scene.h>
#endif // RL_SG_FCL
#ifdef RL_SG_PQP
#include <rl/sg/pqp/Scene.h>
#endif // RL_SG_PQP
#ifdef RL_SG_SOLID
#include <rl/sg/solid/Scene.h>
#endif // RL_SG_SOLID

int main()
{
	/*if (argc < 6)
	{
		std::cout << "Usage: rlEetTest ENGINE SCENEFILE KINEMATICSFILE EXPECTED_NUM_VERTICES_MAX EXPECTED_NUM_EDGES_MAX START1 ... STARTn GOAL1 ... GOALn" << std::endl;
		return EXIT_FAILURE;
	}*/

	
	std::string scene_path = "./CFP/CFP_Scene_1.xml";
	std::string kin_path = "./CFP/kuka-kr60-3.xml";
	
	//定义场景模型
	std::shared_ptr<rl::sg::Scene> scene;
	scene = std::make_shared<rl::sg::bullet::Scene>();

	//读入模型文件
	scene->load(scene_path);
	std::shared_ptr<rl::kin::Kinematics> kinematics(rl::kin::Kinematics::create(kin_path));
	std::vector<rl::math::Real>angles0{ 83.62, -57.27, 90.64, -11.48, -33.9, -80.43 };
	std::vector<rl::math::Real>angles1{ 35.43, -22.37, 60.43, 31.15, -42.46, -204.03 };

	Eigen::Matrix<rl::math::Unit, Eigen::Dynamic, 1> qUnits(kinematics->getDof());
	kinematics->getPositionUnits(qUnits);

	//起点转角
	rl::math::Vector start(kinematics->getDof());

	for (std::size_t i = 0; i < kinematics->getDof(); ++i)
	{
		start(i) = boost::lexical_cast<rl::math::Real>(angles0[i]);

		if (rl::math::UNIT_RADIAN == qUnits(i))
		{
			start(i) *= rl::math::DEG2RAD;
		}
		std::cout << start(i) << "  ";
	}
	std::cout << std::endl;
	//终点转角
	rl::math::Vector goal(kinematics->getDof());

	for (std::size_t i = 0; i < kinematics->getDof(); ++i)
	{
		goal(i) = boost::lexical_cast<rl::math::Real>(angles1[i]);

		if (rl::math::UNIT_RADIAN == qUnits(i))
		{
			goal(i) *= rl::math::DEG2RAD;
		}
		std::cout << goal(i) << "  ";
	}
	std::cout<<std::endl;

	rl::plan::DistanceModel model;
	model.kin = kinematics.get();
	model.model = scene->getModel(0);
	model.scene = scene.get();

	rl::plan::WorkspaceSphereExplorer explorer;
	rl::plan::Eet::ExplorerSetup explorerSetup;
	rl::plan::LinearNearestNeighbors nearestNeighbors(&model);
	rl::plan::Eet planner;
	rl::plan::UniformSampler sampler;

	rl::math::Vector3 explorerGoal;
	rl::math::Vector3 explorerStart;

	explorer.seed(0);
	planner.seed(0);
	sampler.seed(0);

	explorer.goal = &explorerGoal;
	explorer.greedy = rl::plan::WorkspaceSphereExplorer::GREEDY_SPACE;
	explorer.model = &model;
	explorer.radius = 0.025;
	explorer.range = 2.19;
	explorer.samples = 100;
	explorer.start = &explorerStart;

	explorerSetup.goalConfiguration = &goal;
	explorerSetup.goalFrame = -1;
	explorerSetup.startConfiguration = &start;
	explorerSetup.startFrame = -1;

	planner.alpha = 0.01f;
	planner.alternativeDistanceComputation = false;
	planner.beta = 0;
	planner.delta = 1.0f * rl::math::DEG2RAD;
	planner.distanceWeight = 0.1f;
	planner.epsilon = 1.0e-9f;
	planner.explorers.push_back(&explorer);
	planner.explorersSetup.push_back(explorerSetup);
	planner.gamma = 1.0f / 3.0f;
	planner.goal = &goal;
	planner.goalEpsilon = 0.1f;
	planner.goalEpsilonUseOrientation = false;
	planner.max.x() = 2.77634;
	planner.max.y() = 2.5;
	planner.max.z() = 3.5;
	planner.model = &model;
	planner.min.x() = -0.72466;
	planner.min.y() = -2.5;
	planner.min.z() = 0;
	planner.setNearestNeighbors(&nearestNeighbors, 0);
	planner.sampler = &sampler;
	planner.start = &start;

	sampler.model = &model;

	std::cout << "solve() ... " << std::endl;;
	std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now();
	bool solved = planner.solve();
	std::chrono::steady_clock::time_point stopTime = std::chrono::steady_clock::now();
	std::cout << "solve() " << (solved ? "true" : "false") << " " << std::chrono::duration_cast<std::chrono::duration<double>>(stopTime - startTime).count() * 1000 << " ms" << std::endl;

	std::cout << "NumVertices: " << planner.getNumVertices() << "  NumEdges: " << planner.getNumEdges() << std::endl;

	//读取路径
	rl::plan::VectorList paths = planner.getPath();
	rl::plan::VectorList::const_iterator i = paths.begin();
	std::cout << "paths.size() =" << paths.size() << std::endl;
	int t = 0;
	for (; i != paths.end(); ++i)
	{
		rl::math::Vector pos = *i * rl::math::RAD2DEG;
		std::cout << t <<":  "<< pos.transpose() << std::endl;
		t++;
	}


	if (solved)
	{
		/*if (boost::lexical_cast<std::size_t>(argv[4]) >= planner.getNumVertices() &&
			boost::lexical_cast<std::size_t>(argv[5]) >= planner.getNumEdges())*/
		if (120 >= planner.getNumVertices() &&
			100 >= planner.getNumEdges())
		{
			return EXIT_SUCCESS;
		}
		else
		{
			std::cerr << "NumVertices and NumEdges are more than expected for this test case.";
			return EXIT_FAILURE;
		}
	}

	return EXIT_FAILURE;

}

参考:
1.概率路线图(PRM)算法
2.RRT算法

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/19024.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

牧场系统设计与实现-计算机毕业设计源码+LW文档

关键代码 package com.example.controller; import cn.hutool.core.collection.CollUtil; import cn.hutool.core.collection.CollectionUtil; import cn.hutool.core.io.IoUtil; import cn.hutool.core.util.ObjectUtil; import cn.hutool.core.util.StrUtil; import cn.huto…

japonensisjava乱码_Java乱码问题原因及解决方案

一. java乱码乱码问题解决思路,java乱码分为以下几种 1. 请求乱码 如果是在请求过程中发生的乱码,有可能是以下几种情况: 参数放在 URL 地址中乱码; 参数在请求体中乱码; 数据库本身乱码; 数据从 Java 应用程序传入数据库的过程中乱码。 2. 响应乱码 如果是响应乱码,那…

Go语言学习(八)-- Gin入门

Gin 是一个 Go (Golang) 编写的轻量级 http web 框架&#xff0c;运行速度非常快。Gin 最擅长的就是 Api 接口的高并发&#xff0c;如果项目的规模不大&#xff0c;业务相对简单&#xff0c;这个时候我们 也推荐您使用 Gin。 当某个接口的性能遭到较大挑战的时候&#xff0c;这…

Prometheus监控案例

一&#xff1a;环境规划&#xff1a; 主机名主机地址角色node4192.168.188.114prometheus客户端node5192.168.188.115prometheus服务端 二. 监控远程linux主机&#xff1a; 1. 解压node_exporter压缩包&#xff1a; [rootnode4 ~]# tar xf node_exporter-1.3.1.linux-amd64…

RT-Thread 下的文件内容对比 MSH shell cmd 命令实现方法

前言 在使用 RT-Thread 时&#xff0c;需要对两个文件的内容进行比较&#xff0c;顺手写了一个测试的 MSH shell 命令&#xff0c;经过优化&#xff0c;发现功能可以使用 RT-Thread 下支持多种文件系统&#xff0c;如FAT等&#xff0c;可以通过 USB、串口 的 Ymodem 等协议把文…

YOLOv5-seg数据集制作、模型训练以及TensorRT部署

YOLOv5-seg数据集制作、模型训练以及TensorRT部署版本声明一、数据集制作&#xff1a;图像 Json转txt二、分割模型训练三 tensorRT部署版本声明 yolov5-seg:官方地址&#xff1a;https://github.com/ultralytics/yolov5/tree/v6.2 TensorRT&#xff1a;8.x.x 语言&#xff1a;…

基于stm32单片机体重秤电子秤超重提醒

资料编号&#xff1a;107 下面是相关功能视频演示&#xff1a; 107-基于stm32单片机体重秤电子秤称重超重报警Proteus仿真&#xff08;源码仿真全套资料&#xff09;功能介绍&#xff1a; 采用stm32单片机&#xff0c;可以设置称重上限制&#xff0c;LCD1602显示重量&#xf…

04_tcp

知识点1【多播】 多播地址&#xff1a; 多播地址向以太网MAC地址的映射 UDP多播工作过程&#xff1a; 多播地址结构体&#xff1a; 多播套接口选项&#xff1a; 知识点2【TCP面向链接编程】 1、创建tcp套接字 2、做为客户端需要具备的条件 3、connect链接服务器的函数…

地图下载白嫖神器!你该怎么用好它

今天介绍一下做数据可视化网站比较好的两个平台。一个是阿里云的Datav&#xff0c;另一个是易智微easyv. 一、DataV.GeoAtlas 前段时间 我们就给大家分享过阿里云的DataV.GeoAtlas地理小工具系列。我们可以通过这个平台下载高德比较新的地图数据&#xff0c;数据的时效性是有较…

如何查看SAP版本及HANA版本?

目录 一、查SAP NetWeaver版本 二、查看S/4 HANA版本 在SAP运维及系统集成时&#xff0c;经常外面公司问及本公司的SAP版本及HANA版本。其实是每一个登录SAP的用户都可以查到的。方法如下&#xff1a; 一、查SAP NetWeaver版本 SAP界面上选择菜单&#xff1a;系统-状态&am…

哈夫曼树与哈夫曼编码

哈夫曼树&#xff1a;结点中赋予一个某种意义的值&#xff0c;称为结点的权值&#xff0c;从根结点开始&#xff0c;到目标结点经过的边数&#xff0c;称为路径长度&#xff0c;路径长度乘以权值&#xff0c;称为带权路径长度&#xff1b; 例如&#xff1a;根结点代表着快递集…

上位机工业协议-S7COMM

1、S7协议主要针对西门子相关设备通信。先了解基本通信对象、通信环境、通信报文&#xff0c;再处理S7COMM通信库的封装与测试。 2、西门子设备通信 - PLC&#xff1a;系列 LOGO、200、200Smart、300、400、1200、1500 - PLC&#xff1a;LOGO、200、200Smart、300、400、1…

Elastic Stack容器化部署拓展(Https、AD域集成)并收集Cisco设备的日志信息

前言&#xff1a; 还记得在去年的笔记中提到过EFK&#xff08;Elasticsearch-Filebeat-Kibana&#xff09;的部署&#xff0c;但是其中的内容相对简单&#xff0c;也没有提到一些额外的Elastic Stack的特性。链接如下&#xff1a;https://blog.csdn.net/tushanpeipei/article/…

JSTL使用

目录 简介&#xff1a; 组成 使用&#xff1a; code核心库使用 ​编辑 fmt格式化 ​编辑 简介&#xff1a; 全称:JSP Standard Tag Library 中文名:JSP标准标签库 作用:用于扩展JSP中的标签&#xff0c;能够为JSP页面提供流程控制、类型转换等功能的标签。替换JSP中代码…

【Spring Cloud实战】Ribbon负载均衡

gitee地址&#xff1a;https://gitee.com/javaxiaobear/spring-cloud_study.git 在线阅读地址&#xff1a;https://javaxiaobear.gitee.io/ 1、概述 Spring Cloud Ribbon是基于Netflix Ribbon实现的一套客户端负载均衡的工具。 简单的说&#xff0c;Ribbon是Netflix发布的开源项…

jenkins持续集成 自动化部署

一、环境准备 1.1 Java环境 &#xff08;1&#xff09;安装jdk1.8 yum -y install java-1.8.0-openjdk* &#xff08;2&#xff09;执行以下命令查看是否安装成功 java -version 1.2 安装maven &#xff08;1&#xff09;将安装包上传到Linux服务器&#xff0c;解压缩 tar -…

对笔记本电池的研究

文章目录设计容量&完全充电容量笔记本电池报告显示电池设计与系统电池的全部充电容量之间的差异解释电池损耗正确做法查看笔记本的电池使用报告方法第一步&#xff1a;WinR键输入cmd&#xff0c;打开命令提示符窗口第二步&#xff1a;输入powercfg /batteryreport&#xff…

代码规范-对抗软件复杂度

1、为什么需要代码规范 任何系统性的项目都需要架构设计&#xff0c;而架构设计的核心命题是控制复杂度。 但随着项目的不断迭代&#xff0c;复杂度就会不断上升&#xff0c;研发效率就会不断下降。 而代码规范正是对抗软件复杂度的有效手段&#xff0c;通过约定俗成的规则…

[附源码]计算机毕业设计JAVA户籍管理系统

[附源码]计算机毕业设计JAVA户籍管理系统 项目运行 环境配置&#xff1a; Jdk1.8 Tomcat7.0 Mysql HBuilderX&#xff08;Webstorm也行&#xff09; Eclispe&#xff08;IntelliJ IDEA,Eclispe,MyEclispe,Sts都支持&#xff09;。 项目技术&#xff1a; SSM mybatis M…

docker安装redis详细教程

1、下载最新redis镜像 docker pull redis //表示拉取最新的镜像 如果要指定版本 docker pull redis:latest//表示拉取最新的镜像 2、创建redis映射目录 mkdir /redisData/redis/conf 配置文件挂载在我指定的redisData/redis/conf/ 文件夹中&#xff0c;方便后续的修改 创建re…