（9-3-1）RRT算法：机器人路径规划和轨迹优化系统

最新推荐文章于 2024-04-13 09:39:55 发布

码农三叔

最新推荐文章于 2024-04-13 09:39:55 发布

阅读量1.7k

点赞数 20

分类专栏：路径规划与决策算法应用与实战(C++卷) 文章标签：算法机器人 c++

本文链接：https://blog.csdn.net/asd343442/article/details/136556736

版权

路径规划与决策算法应用与实战(C++卷) 专栏收录该内容

41 篇文章 9 订阅

订阅专栏

9.3 机器人路径规划和轨迹优化系统

本项目实现了路径规划和轨迹优化功能。首先，使用RRT*算法在给定空间中搜索最优路径，考虑了障碍物避障等因素。然后，利用最小加速度捆绑（Minimum Snap）算法对路径进行优化，生成平滑的轨迹。最后，通过MATLAB可视化工具绘制了优化后的三维路径。

9.3.1 项目介绍

随着机器人技术的快速发展，机器人在各个领域的运用越来越广泛，其中路径规划和轨迹优化是关键的技术之一。针对复杂环境中的机器人运动问题，需要高效的路径规划算法和轨迹优化方法来保证机器人的安全性和效率性。因此，开发一个综合的机器人路径规划和轨迹优化系统具有重要意义。

本项目是一个基于C++和MATLAB实现的综合性项目，旨在提供一套完整的解决方案，用于解决机器人在复杂环境中的路径规划和轨迹优化问题。该系统包括两个核心模块：RRTStar和MinSnapTraj。RRTStar模块实现了快速探索随机采样空间的路径规划算法，能够在给定约束条件下寻找到高效的路径。而MinSnapTraj模块则专注于对生成的路径进行轨迹优化，利用最小加速度平滑轨迹生成算法，实现对机器人运动轨迹的优化和平滑化。通过这两个模块的协同工作，系统能够为各类机器人提供高效、安全的路径规划和轨迹优化方案，为机器人在复杂环境中的运动提供可靠的支持。

9.3.2 实现RRT* 算法

在本项目中，文件RRTStar.h 提供了 RRT* 算法的声明，包括类的成员变量和方法，用于在 C++ 中实现 RRT* 算法。这个头文件定义了 RRTStar 类，其中包含了算法所需的参数和方法的声明，以及用于执行 RRT* 算法的函数原型。文件RRTStar_withHeader.cpp 实现了 RRTStar.h 中声明的方法，完成了 RRT* 算法的具体逻辑。这个文件包含了 RRTStar 类的成员函数的实现，实现了 RRT* 算法的核心功能，包括树的构建、路径搜索、路径优化等功能，并提供了用于单元测试的示例代码。

（1）文件RRTStar.h是一个头文件，其中只包含了 RRTStar 类的声明，没有实际的实现。在 C++ 中，头文件通常用于声明类、函数、变量等，而具体的实现则在对应的源文件中。

#ifndef RRTSTAR_H
#define RRTSTAR_H

#include <iostream>
#include <vector>
#include <cmath>
#include <unordered_map>
#include <map>
#include <random>
#include <algorithm>
#include <fstream>
#include <string>
#include <stdexcept>

class RRTStar {
public:
    std::vector<double> start;
    std::vector<double> goal;
    std::vector<double> goalError;
    int maxIterations;
    std::vector<std::vector<double>> obstacles;
    double maxDistance;
    std::vector<double> lower;
    std::vector<double> upper;
    double stepSize;
    double neighbourRadius;
    double epsilon;
    std::vector<double> currSample;
    int dynamic_it_counter;
    int dynamic_break_at;
    std::vector<std::vector<double>> spaceLimits;

    std::vector<std::vector<double>> allNodes;
    std::vector<std::vector<double>> bestPath;
    std::map<std::string, std::vector<double>> bestTree;
    std::map<std::string, std::vector<double>> tree;

    RRTStar(std::vector<std::vector<double>> spaceLimits, std::vector<double> start, std::vector<double> goal, double max_distance, int maxIterations, std::vector<std::vector<double>> obstacle);

    void updateTree(std::vector<double> node, std::vector<double> newNode);
    std::vector<double> generateNode();
    std::vector<double> findNearest(std::vector<double> newNode);
    std::vector<double> unitVectorToNode(std::vector<double> newNode, std::vector<double> nearestNode);
    std::vector<std::vector<double>> validNeighbours(std::vector<double> newNode);
    bool validConnection(std::vector<double> node, std::vector<double> newNode);
    std::vector<double> bestNeighbour(std::vector<std::vector<double>> neighbours);
    bool rewire(std::vector<std::vector<double>> neighbours, std::vector<double> newNode);
    bool isPathFound(std::map<std::string, std::vector<double>> tree, std::vector<double> newNode);
    std::vector<std::vector<double>> getPath(std::map<std::string, std::vector<double>> tree);
    void store_best_tree();
    static double path_cost(std::vector<std::vector<double>> path);
    void run();
    void writeDataToCSV(const std::vector<std::vector<double>>& data, const std::string& filename);
};

#endif // RRTSTAR_H

在上述代码中声明了如下所示的成员变量。

start：起始点的坐标。
goal：目标点的坐标。
goalError：目标误差，用于判断是否到达目标。
maxIterations：最大迭代次数。
obstacles：障碍物的坐标。
maxDistance：每次扩展的最大距离。
lower、upper：状态空间的下界和上界。
stepSize：步长。
neighbourRadius：邻居半径。
epsilon：阈值，用于判断两个节点是否相等。
currSample：当前采样的点。
dynamic_it_counter、dynamic_break_at：用于动态设置迭代次数。
spaceLimits：状态空间的限制。

（2）文件RRTStar_withHeader.cpp 包含了对前面的类RRTStar 类成员函数的具体实现，这些函数包括构造函数、更新树、生成节点、寻找最近节点、计算单位向量、获取有效邻居节点、判断连接是否有效、获取最佳邻居、重新连接节点、判断路径是否找到、获取路径、存储最佳树、计算路径成本、运行RRT*算法和将数据写入CSV文件。

构造函数RRTStar::RRTStar负责初始化 RRTStar 类的实例，包括设置起始点、目标点、状态空间限制、最大迭代次数和障碍物等参数。它还计算目标误差，用于判断是否到达目标，以及将起始点添加到节点列表中。

RRTStar::RRTStar(vector<vector<double>> spaceLimits, vector<double> start, vector<double> goal, double max_distance, int maxIterations, vector<vector<double>> obstacle){
        this->start = start;
        this->goal = goal;
        this->goalError = {goal[0] - 0.05, goal[1] - 0.05, goal[2]-0.05};
        this->maxIterations = maxIterations;
        this->maxDistance = max_distance;
        this->obstacles = obstacle;
        this->lower = spaceLimits[0];
        this->upper = spaceLimits[1];
        this->allNodes.push_back(this->start);

        this->stepSize = max_distance;
        this->neighbourRadius = 1.1 * this->maxDistance;
        this->epsilon = 0.3;
        this->currSample = {};

        this->bestPath = {};
        this->bestTree = {};

        this->dynamic_it_counter = 0;
        this->dynamic_break_at = this->maxIterations / 10;

}

函数RRTStar::updateTree 的功能是更新 RRT 树，将新的节点添加到树中，并指定其父节点。具体来说，它接受两个参数：要添加到树中的新节点和该新节点的父节点。然后，它将新节点添加到所有节点列表中，并使用字符串键值表示新节点，将其与其父节点关联起来，以便构建整棵树的结构。

void RRTStar::updateTree(vector<double> node, vector<double> newNode){
        this->allNodes.push_back(newNode);
        std::string key = to_string(newNode[0]) + "," + to_string(newNode[1]) + "," + to_string(newNode[2]);
        std::vector<double> parent = node;
        if(newNode != node){
            this->tree[key] = parent;
        }
}

函数RRTStar::generateNode() 的功能是生成一个新的随机节点，用于扩展 RRT 树。它会根据状态空间的限制和一定的随机性，随机生成一个新的节点。如果随机数小于预先定义的目标到达概率阈值（epsilon），则返回目标节点，否则在状态空间范围内生成一个随机节点，并将其四舍五入到两位小数。生成的随机节点将作为 RRT 树的扩展目标。

std::vector<double> RRTStar::generateNode(){
        random_device rd;
        mt19937 gen(rd());
        uniform_real_distribution<double> dis(0.0, 1.0);
        if(dis(gen) < this->epsilon){
            return this->goal;
        }

        double x = (dis(gen) * (this->upper[0] - this->lower[0])) + this->lower[0];
        double y = (dis(gen) * (this->upper[1] - this->lower[1])) + this->lower[1];
        double z = (dis(gen) * (this->upper[2] - this->lower[2])) + this->lower[2];

        std::vector<double> randomNode = {std::round(x*100)/100, round(y*100)/100, round(z*100)/100};
        return randomNode;
}

函数RRTStar::findNearest(vector<double> newNode) 的功能是找到 RRT 树中离给定节点（newNode）最近的节点。它遍历所有已知节点，并计算它们与给定节点的距离，然后返回距离最近的节点作为结果。这个函数帮助确定在 RRT 树中哪个节点最适合作为新节点的父节点，以便在树中扩展。

std::vector<double> RRTStar::findNearest(vector<double> newNode){
        std::vector<double> distances;
        for(const auto& node : allNodes){
            distances.push_back(sqrt(pow(newNode[0] - node[0],2) + pow(newNode[1] - node[1],2) + pow(newNode[2] - node[2],2)));
        }

        std::vector<double> nearestNode = allNodes[distance(distances.begin(), min_element(distances.begin(), distances.end()))];
        return nearestNode;

}

函数RRTStar::unitVectorToNode(vector<double> newNode, vector<double> nearestNode) 的功能是计算从最近节点（nearestNode）到新节点（newNode）的单位向量。它首先计算两个节点之间的距离，然后根据距离调整新节点的位置，使得新节点沿着与最近节点相连的方向移动一个单位距离。最后，它将调整后的新节点作为结果返回。这个函数在 RRT* 算法中用于确定如何将新节点添加到树中。

std::vector<double> RRTStar::unitVectorToNode(vector<double> newNode, vector<double> nearestNode){
        double distance_nearest = std::sqrt(std::pow(newNode[0] - nearestNode[0], 2) + std::pow(newNode[1] - nearestNode[1], 2) + std::pow(newNode[2] - nearestNode[2], 2));
        if (distance_nearest > this->stepSize) {
            double x = nearestNode[0] + (newNode[0] - nearestNode[0]) * this->stepSize / distance_nearest;
            double y = nearestNode[1] + (newNode[1] - nearestNode[1]) * this->stepSize / distance_nearest;
            double z = nearestNode[2] + (newNode[2] - nearestNode[2]) * this->stepSize / distance_nearest;
            std::vector<double> new_node = {std::round(x*100)/100, std::round(y*100)/100, std::round(z*100)/100};
            newNode = new_node;
        }
        return newNode;
}

函数RRTStar::validNeighbours(vector<double> newNode) 的功能是获取给定节点周围有效的邻居节点。它遍历所有已知节点，并计算它们与给定节点的距离，然后筛选出距离在邻居半径内的节点。对于每个满足条件的节点，还会检查与给定节点之间是否存在有效的连接（即路径上是否有障碍物）。最终，返回所有有效邻居节点的列表。这个函数在 RRT* 算法中用于确定哪些节点可以作为新节点的邻居，以便在树中进行扩展。

std::vector<std::vector<double>> RRTStar::validNeighbours(vector<double> newNode){
        std::vector<std::vector<double>> neighbours;
        for (std::vector<double> node : this->allNodes) {
            double distance = std::sqrt(std::pow(node[0] - newNode[0], 2) + std::pow(node[1] - newNode[1], 2) + std::pow(node[2] - newNode[2], 2));
            if (distance <= this->neighbourRadius) {
                if (this->validConnection(node, newNode)) {
                    neighbours.push_back(node);
                }
            }
        }
        return neighbours;
}

函数RRTStar::validConnection(std::vector<double> node, std::vector<double> newNode) 的功能是判断两个节点之间是否存在有效的连接，即路径上是否有障碍物。它接受两个节点作为参数，并检查它们之间的直线路径是否与任何障碍物相交。如果路径上没有障碍物，则返回 true，表示节点之间的连接有效；否则返回 false，表示节点之间的连接存在障碍物，不可通过。这个函数在 RRT* 算法中用于检查节点之间的连接是否合法，以便确定是否将新节点添加到树中。

bool RRTStar::validConnection(std::vector<double> node, std::vector<double> newNode){
        if (this->obstacles.empty()) {
            return true;
        }
        for (std::vector<double> obs : this->obstacles) {
            double xmin = obs[0];
            double xmax = obs[1];
            double ymin = obs[2];
            double ymax = obs[3];
            double zmin = obs[4];
            double zmax = obs[5];
            std::vector<double> node1 = node;
            std::vector<double> node2 = newNode;
            std::vector<double> direction = {node2[0] - node1[0], node2[1] - node1[1], node2[2] - node1[2]};
            std::vector<double> t(100);
            std::vector<std::vector<double>> points(100, std::vector<double>(3));
            for (int i = 0; i < 100; i++) {
                t[i] = i / 99.0;
                points[i][0] = node1[0] + t[i] * direction[0];
                points[i][1] = node1[1] + t[i] * direction[1];
                points[i][2] = node1[2] + t[i] * direction[2];
            }
            for (std::vector<double> point : points) {
                if (point[0] >= xmin && point[0] <= xmax && point[1] >= ymin && point[1] <= ymax && point[2] >= zmin && point[2] <= zmax) {
                    return false;
                }
            }
        }
        return true;
}

函数RRTStar::bestNeighbour(std::vector<std::vector<double>> neighbours) 的功能是从一组邻居节点中选择最佳的节点作为当前节点的邻居，它接受一个邻居节点的列表作为参数，并计算每个邻居节点到起始点的代价（通常是欧几里得距离）。然后，它选择代价最低的邻居节点作为最佳邻居，并将其返回。这个函数在 RRT* 算法中用于确定哪个邻居节点最适合作为当前节点的父节点，以便在树中扩展。

std::vector<double> RRTStar::bestNeighbour(std::vector<std::vector<double>> neighbours){

        double min_cost = std::numeric_limits<double>::max();
        std::vector<double> best_neighbour;
        for (std::vector<double> neighbour : neighbours) {
            double cost = std::sqrt(std::pow(neighbour[0] - this->start[0], 2) + std::pow(neighbour[1] - this->start[1], 2) + std::pow(neighbour[2] - this->start[2], 2));
            if (cost < min_cost) {
                min_cost = cost;
                best_neighbour = neighbour;
            }
        }
        return best_neighbour;
}

函数RRTStar::rewire(std::vector<std::vector<double>> neighbours, std::vector<double> newNode) 的功能是尝试重新连接树中的某些节点，以改善树的结构和性能。它接受一组邻居节点和一个新节点作为参数，并尝试重新连接这些邻居节点到新节点，以便通过新节点到达树中其他节点。在重新连接过程中，它会检查新连接路径是否比现有路径更短且没有穿过障碍物。如果重新连接成功，则更新树的连接关系，并返回 true；否则返回 false。这个函数在 RRT* 算法中用于优化树的结构，以确保树的扩展路径是最优的。

bool RRTStar::rewire(std::vector<std::vector<double>> neighbours, std::vector<double> newNode){
        for(std::vector<double> neigh : neighbours){
            if(neigh ==  this->tree[to_string(round(newNode[0]*100)/100) + "," + to_string(round(newNode[1]*100)/100) + "," + to_string(round(newNode[2]*100)/100)]){
                continue;
            }

            if(this->validConnection(neigh, newNode)){
                std::vector<double> currentParent = this->tree[to_string(round(neigh[0]*100)/100) + "," + to_string(round(neigh[1]*100)/100) + "," + to_string(round(neigh[2]*100)/100)];

                double currentCost = sqrt(pow(neigh[0]-this->start[0],2) + pow(neigh[1]-this->start[1],2) + pow(neigh[2]-this->start[2],2));
                
                double interCost = sqrt(pow(newNode[0]-this->start[0],2) + pow(newNode[1]-this->start[1],2) + pow(newNode[2]-this->start[2],2));
                
                double newCost = sqrt(pow(neigh[0]-newNode[0],2) + pow(neigh[1]-newNode[1],2) + pow(neigh[2]-newNode[2],2)) + interCost;

                if(newCost < currentCost){
                    this->tree[to_string(round(neigh[0]*100)/100) + "," + to_string(round(neigh[1]*100)/100) + "," + to_string(round(neigh[2]*100)/100)] = newNode;
                    return true;
                }
            }
        }

        return false;
}

函数RRTStar::isPathFound(map<string, vector<double>> tree, vector<double> newNode)的功能是检查是否已找到从起始点到达给定节点的路径，它接受一个表示树的映射和一个新节点作为参数，并检查树中是否存在一条从起始点到达新节点的路径。如果存在这样的路径，则返回 true，表示已找到路径；否则返回 false，表示尚未找到路径。这个函数在 RRT* 算法中用于确定是否已找到从起始点到目标点的可行路径。

bool RRTStar::isPathFound(map<string, vector<double>> tree, vector<double> newNode){
        std::string goal_node_key = std::to_string(std::round(this->goal[0]*100)/100) + "," + std::to_string(std::round(this->goal[1]*100)/100) + "," + std::to_string(std::round(this->goal[2]*100)/100);
        return tree.find(goal_node_key) != tree.end();
        
}

函数RRTStar::getPath(map<string, vector<double>> tree)的功能是从树中重建路径。它接受一个表示树的映射作为参数，并从树的结构中重建从起始点到目标点的路径。具体来说，它从目标节点开始，沿着每个节点的父节点向上回溯，直到回溯到起始点，形成完整的路径。然后，它将路径以节点列表的形式返回。这个函数在 RRT* 算法中用于从构建好的树结构中提取最优路径。

std::vector<std::vector<double>> RRTStar::getPath(map<string, vector<double>> tree){
        std::vector<std::vector<double>> path;
        std::vector<double> node = this->goal;
        while (node != this->start) {
            path.push_back(node);
            node = tree[std::to_string(std::round(node[0]*100)/100) + "," + std::to_string(std::round(node[1]*100)/100) + "," + std::to_string(std::round(node[2]*100)/100)];
        }
        std::reverse(path.begin(), path.end());
        return path;
}

函数RRTStar::store_best_tree() 的功能是将当前树结构保存为最优树结构。它将当前的树结构（通常是已经找到的最优路径）存储在类成员变量 bestTree 中，以便稍后进行访问和使用。这个函数通常在找到更好的路径后被调用，以更新最优路径的表示。在 RRT* 算法中，它用于保存找到的最佳路径的树结构。

void RRTStar::store_best_tree() {
        this->bestTree = this->tree;
}

函数RRTStar::path_cost(vector<vector<double>> path) 的功能是计算给定路径的总代价，它接受一个表示路径的二维向量作为参数，并计算路径上相邻节点之间的欧几里得距离之和，作为路径的总代价。最后，它返回路径的总代价。这个函数在 RRT* 算法中用于评估路径的质量和效果。

double RRTStar::path_cost(vector<vector<double>> path){
        double cost = 0;
        for(int i=0; i<path.size()-1; i++){
            vector<double> node1 = path[i+1];
            vector<double> node2 = path[i];
            cost += sqrt(pow(node1[0]-node2[0],2) + pow(node1[1]-node2[1],2) + pow(node1[2]-node2[2],2));
        }
        return cost;
}

函数RRTStar::run() 是执行 RRT* 算法的核心方法。它负责根据给定的起始点、目标点和障碍物，在给定的空间范围内运行 RRT* 算法来搜索从起始点到目标点的最优路径。这个函数是整个 RRT* 算法的执行入口，它通过迭代优化树结构来搜索最优路径，并在找到路径或达到最大迭代次数时终止算法。

void RRTStar::run(){
        double old_cost = std::numeric_limits<double>::infinity();
        for(int i=0; i<this->maxIterations; i++){
            std::vector<double> node = this->generateNode();
            std::vector<double> neartestNode = this->findNearest(node);
             this->currSample = this->unitVectorToNode(node, neartestNode);
            std::vector<std::vector<double>> neighbours = this->validNeighbours(this->currSample);
            if(neighbours.empty()){
                continue;
            }

            std::vector<double> best_Neighbour = bestNeighbour(neighbours);
            this->updateTree(best_Neighbour, this->currSample);
            bool hasRewired = this->rewire(neighbours, this->currSample);
            if(this->isPathFound(this->tree, this->currSample)){
                cout<<"here in run if path found"<<endl;
                std::vector<std::vector<double>> path = this->getPath(this->tree);
                //double cost = result.second;
                this->store_best_tree();
                std::cout << "iterations:"<< i <<std::endl;
                break;
            }
        }

        if (!this->isPathFound(this->bestTree, this->currSample)) {
            throw std::runtime_error("No path found");
        }
        this->bestPath = this->getPath(this->bestTree);
        //this->bestPath = result1.first;
        std::cout << "Best path found with cost: " << RRTStar::path_cost(this->bestPath) << std::endl;
}

函数RRTStar::writeDataToCSV(const std::vector<std::vector<double>>& data, const std::string& filename) 的功能是将数据写入 CSV 文件，它接受一个二维向量 data，表示要写入文件的数据，以及一个字符串 filename，表示要写入的文件名。这个函数通常在 RRT* 算法找到最优路径后，将最优路径的节点坐标写入到 CSV 文件中，以便后续进行可视化或其他处理。

void RRTStar::writeDataToCSV(const std::vector<std::vector<double>>& data, const std::string& filename) {
        std::ofstream outputFile(filename);

        if (outputFile.is_open()) {
            for (const auto& row : data) {
                for (size_t i = 0; i < row.size(); ++i) {
                    outputFile << row[i];
                    if (i < row.size() - 1) {
                        outputFile << ",";  // Add a comma between values
                    }
                }
                outputFile << "\n";  // Start a new line for each row
            }

            outputFile.close();
            std::cout << "Data has been written to " << filename << std::endl;
        } else {
            std::cerr << "Unable to open file: " << filename << std::endl;
        }
}

下面这段代码是用于单元测试的示例代码，用于测试 RRTStar 类的功能是否正确。在这段代码中，首先创建了一个 RRTStar 类的对象 rrt，然后调用了 run() 方法运行 RRT* 算法以搜索最优路径。接着，将找到的最优路径写入到名为 "output.csv" 的 CSV 文件中，以便后续的分析和可视化。最后，函数返回 0，表示程序正常结束。

// Uncomment for Unit Testing of Code

// int main() {
//     std::vector<double> start = {0, 0, 0};
//     std::vector<double> goal = {7.0*10, 7.0*10, 7.0*10};
//     std::vector<std::vector<double>> space_limits = {{0., 0., 0.9}, {100., 100., 100.}};
//     RRTStar rrt(space_limits, start, goal, 5, 1000, {{15, 55, 15, 55, 15, 55}});
//     rrt.run();

//     std::string filename = "output.csv";
//     rrt.writeDataToCSV(rrt.bestPath, filename);
//     //rrt.plot();
//     return 0;
// }

注意：在实际应用中，可以根据需要选择是否解除注释这段代码，并根据具体情况修改起始点、目标点、空间范围和障碍物等参数，以便进行单元测试和验证算法的正确性。

未完待续

码农三叔

关注

20
点赞
踩
19

收藏

觉得还不错? 一键收藏
打赏
0
评论
（9-3-1）RRT算法：机器人路径规划和轨迹优化系统

随着机器人技术的快速发展，机器人在各个领域的运用越来越广泛，其中路径规划和轨迹优化是关键的技术之一。针对复杂环境中的机器人运动问题，需要高效的路径规划算法和轨迹优化方法来保证机器人的安全性和效率性。因此，开发一个综合的机器人路径规划和轨迹优化系统具有重要意义。本项目是一个基于C++和MATLAB实现的综合性项目，旨在提供一套完整的解决方案，用于解决机器人在复杂环境中的路径规划和轨迹优化问题。该系统包括两个核心模块：RRTStar和MinSnapTraj。
复制链接

扫一扫