(二) 路径规划算法---RRT_Star结合OMPL实现三维点云的路径规划

最新推荐文章于 2026-04-18 02:49:02 发布

原创

最新推荐文章于 2026-04-18 02:49:02 发布 · 7.3k 阅读

106

标签

#算法 #自动驾驶 #人工智能

本文介绍了如何利用OMPL优化库在三维点云环境中集成RRT_Star算法，详细步骤包括OMPL的安装、状态空间构建、参数设置和路径求解，最后提供了完整的代码示例和编译指导。

RRT_Star结合OMPL实现三维点云的路径规划

文章目录

RRT_Star结合OMPL实现三维点云的路径规划

OMPL中文名为最优规划，比如

RRT^{\ast}

。采用OMPL为基本框架，在三维点云中实现

RRT^{\ast}

算法。

1.OMPL安装

网址：https://ompl.kavrakilab.org/download.html

推荐使用脚本安装，下载脚本后

sudo chmod 777 install-ompl-ubuntn.sh
./install-ompl-ubuntn.sh

在这里插入图片描述

注意：1.如果在安装脚本的过程中，需要下载ros中的一些东西，此时最好在安装角本前，更换ros源。 ros源更换参考：ROS换源（除了清华之外的ROS源）
ROS wiki

2.安装ompl可能会出现电脑分辨率的发生变化，并且此时会弹出Patch the file（我记得是这样），此时先不要管，一路回车即可。最终再在电脑设置中更改分辨率。

2.OMPL使用

2.1创建机器人的状态空间

因为是在有限点云空间中进行路径规划，因此机器人的状态空间为3，其次需要为空间设置一个边界。

ob::StateSpacePtr space(new ob::RealVectorStateSpace(3));

//将空间的边界设置为[0,1]。
ob::RealVectorBounds bounds(3);
bounds.setLow(0, - _x_size * 0.5);
bounds.setLow(1, - _y_size * 0.5);
bounds.setLow(2, 0.0);

bounds.setHigh(0, + _x_size * 0.5);
bounds.setHigh(1, + _y_size * 0.5);
bounds.setHigh(2, _z_size);
space->as<ob::RealVectorStateSpace>()->setBounds(bounds);

2.2 构造一个空间实例

ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
//设置用于检查空间中哪些状态有效的对象
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
si->setup();

其中ValidityChecker为自定义的类，目的时继承ompl下的StateValidityChecker，并重写有效性函数isValid（ompl无法对该点状态做有效性（安全性）判断）。

class ValidityChecker : public ob::StateValidityChecker
{
   
   
public:
    ValidityChecker(const ob::SpaceInformationPtr& si) :ob::StateValidityChecker(si) {
   
   }
    //返回给定状态的位置是否与圆形障碍物重叠
    bool isValid(const ob::State* state) const
    {
   
      
        //在本例中使用的是RealVectorStateSpace，因此需要将状态转换为特定类型。
        const ob::RealVectorStateSpace::StateType* state3D =state->as<ob::RealVectorStateSpace::StateType>();
        auto x=(*state3D)[0];
        auto y=(*state3D)[1];
        auto z=(*state3D)[2];
        return _RRTstar_preparatory->isObsFree(x, y, z);
    }
};

2.3 创建问题实例，并设置一些参数

auto pdef(std::make_shared<ob::ProblemDefinition>(si));
//设置开始和目标状态
pdef->setStartAndGoalStates(start, goal);
//设置优化目标
pdef->setOptimizationObjective(getPathLengthObjective(si));

其中getPathLengthObjective函数为自定义函数，目的是为ompl提供优化阈值。如果阈值为0.0，表示希望能够找到最好的路线，不设置默认为0.0。阈值通过**obj->setCostThreshold(ob::Cost())**进行设置。

ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si)
{
   
   
     ob::OptimizationObjectivePtr obj(new ob::PathLengthOptimizationObjective(si));
     #obj->setCostThreshold(ob::Cost(1.51));
     return obj;
}

2.4 构建优化器

使用RRT算法构建优化器

ob::PlannerPtr optimizingPlanner(new og::RRTstar(si));
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();

2.5 ompl进行求解

其中参数1.0表示希望在1s中得出路径规划结果

ob::PlannerStatus solved = optimizingPlanner->solve(1.0);

2.6 ompl路径生成

if (solved)
{
   
   
    //从问题定义中获取目标表示（与目标状态不同），并查询找到的路径
    og::PathGeometric* path = pdef->getSolutionPath()->as<og::PathGeometric>();
    vector<Vector3d> path_points;
    for (size_t path_idx = 0; path_idx < path->getStateCount (); path_idx++)
    {
   
   
        const ob::RealVectorStateSpace::StateType *state = path->getState(path_idx)->as<ob::RealVectorStateSpace::StateType>(); 
        auto x = (*state)[0];
        auto y = (*state)[1];
        auto z = (*state)[2];
        Vector3d temp_mat(x,y,z);
        path_points.push_back(temp_mat); 
    }
}

3.整体代码展示

3.1 代码框架修改

在章的基础上进行修改，修改如下

在src文件夹下，新建rrt.cpp,rrt_demo.cpp

在include文件夹下，新建rrt.h

在launch文件夹下，新建rrt_demo.launch

修改CMakeLists.txt内容

3.1.1 rrt.h

#ifndef _GRID_SEARCHER_H_
#define _GRID_SEARCHER_H_

#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/Eigen>
#include "backward.hpp"
#include "node.h"

class RRTstarPreparatory
{
   
   	
	private:

	protected:
		uint8_t * data;
		GridNodePtr *** GridNodeMap;

		int GLX_SIZE, GLY_SIZE, GLZ_SIZE;
		int GLXYZ_SIZE, GLYZ_SIZE;

		double resolution, inv_resolution;
		double gl_xl, gl_yl, gl_zl;
		double gl_xu, gl_yu, gl_zu;	

		Eigen::Vector3d

最低0.47元/天解锁文章