路径规划与优化学习系列（一）—路径规划算法

路径规划与优化学习系列（一）—路径规划算法

前言

几个月来浑浑噩噩，人生这张地图实在太大了，顿时觉得人生之路障碍重重、迷茫不清，故此受人启发，一学路径规划之法，以解心头之困，以便找到寻找最优之人生路（ 结尾有人生之悟）

本文仅用于笔记和回顾，学习来源如下介绍

学习目标

1. 入门路径规划思想
2. 深入了解经典路径规划思想
3. 深入体会各种经典算法的适用场景
4. 深入理解经典算法的代码实现
5. 泛读论文，探索算法改进点

学习来源

1. B站（IR艾若机器人）机器人路径规划、轨迹优化系列课程_哔哩哔哩_bilibili
2. 中国知网、SCI、ieee论文库

正文

一、路径规划思想概述

​ 近年来，路径规划算法高效发展，得到广泛应用。以无人机路径规划为例，主要包括

1.基于飞行区域

• PRM概率路径图法（避免碰撞）
• 分区规划+K-means+模拟退火（复杂约束下多任务多机）
• 人工势场（解决目的点无法准确到达）
• A*定长搜索（起始点确切路径）
• voronoi加权方向图+蚁群算法（高效避障）

2.基于任务时间

• 时间最优+服务结点最大化
• 通信优化框架

3.基于能耗优化

• A*+粒子群优化（路线最优、降低能耗）
• 多机联合+比特分配（满足quality并且降低能耗）

4.基于任务分配

• 动态贝叶斯网络架构（动态飞行复杂环境中的自主飞行和任务执行）
• 异构无人机与多样化任务适配（自然灾害和环境多变下的飞行）

参考：无人机路径规划算法研究_魏涛 DIO:10.27675/d.cnki.gcydx.2020.000204

二、路径规划经典算法

（一）基于搜索的路径规划

1.Dijkstra

背景及适用场景

• 从起始点到终点的最短（最优）路径问题
• 广度优先搜索解决赋权有向图或者无向图的单源最短路径问题，最终得到一个最短路径树。该算法常用于路径搜索或者作为其他图算法的一个子模块
• 本算法也可适用于寻优问题

原理：贪心思想

从起点开始逐步扩展，每一步为一个节点找到最短路径

栅格地图

• 定义：地图划分为若干分辨率的小方格，每个小方格有不同的权值（颜色），代表不同的意义，如下：
  
• 栅格地图的优势：
• 可以将任意形状轮廓的地图，用足够精细的栅格进行绘制
• 每一个栅格，可以通过不同颜色表示不同含义
• 基于栅格地图进行路径规划有横、纵、斜三个规划方向。对应室内低速机器人可以完全按照路径行走；对于中高速机器人，可以考虑将路径进行平滑处理，以适用于非完全约束系统。
  

• matlab绘制栅格地图

% MATLAB 绘制栅格地图的核心函数和思想
% colormap：为栅格地图创建自定义颜色。如黄色栅格代表的起点，红色栅格代表的终点,黑色栅格代表的障碍物
% sub2ind：将行列索引转为线性索引  ind2sub：将线性索引转为行列索引
% image：利用colormap建立的颜色图，将数组信息显示为图像
clc;
clear;
close all;
cmap = [1 1 1; ...       % 1-白色-空地
    0 0 0; ...           % 2-黑色-静态障碍
    1 0 0; ...       % 3-红色-动态障碍
    1 1 0; ...           % 4-黄色-起始点
    1 0 1; ...           % 5-品红-目标点
    0 1 0; ...           % 6-绿色-到达目标点的规划路径
    0 1 1];              % 7-青色-动态规划的路径

% 构建颜色map图
colormap(cmap);

%% 构建栅格地图场景
% 栅格地图界面大小：行数和列数
rows = 10;
cols = 20;
% 定义栅格地图的全域，并初始化空白地图
field = ones(rows,cols);

% 障碍物区域
obsRate = 0.3;
obsNum  = floor(rows * cols * obsRate); % 取整
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;

% 起始点和目标点
startPos = 2;
goalPos = rows * cols - 2;
field(startPos) = 4;
field(goalPos) = 5;

%% 画栅格图
image(1.5,1.5,field);
grid on;
set(gca , 'gridline' , '-' , 'gridcolor' , 'k' , 'linewidth' , 2 , 'GridAlpha' , 0.5);
set(gca , 'xtick' , 1 : cols + 1,'ytick',1 : rows + 1);
axis image;

具体实现

取自：IR艾若机器人

; 2.A*

背景及适用场景

• 从起始点到终点的最短（最优）路径问题
• 广度优先搜索:BFS以起点A为圆心，先搜索A周围的所有点，形成一个类似圆的搜索区域，再扩大搜索半径，进一步搜索其它没搜索到的区域，直到终点B进入搜索区域内被找到
• 深度优先搜索:DFS则是让搜索的区域离A尽量远，离B尽量近

原理：启发式函数

• A算法的导出 首先，BFS保证的是从起点到达路线上的任意点花费的代价最小（但是不考虑这个过程是否要搜索很多格子）。其次，DFS保证的是通过不断矫正行走方向和终点的方向的关系，使发现终点要搜索的格子更少（但是不考虑这个过程是否绕远）。 因此，A算法的设计同时融合了BFS和DFS的优势，既考虑到了从起点通过当前路线的代价（保证了不会绕路），又不断的计算当前路线方向是否更趋近终点的方向（保证了不会搜索很多图块），是一种静态路网中最有效的直接搜索算法。
• 启发式函数

设定地图为栅格地图，运动方向有八个，每个方向都有对应的代价

• F(n)为总代价函数
• g(n)为起点移动到指定结点（当前结点）的代价值
• h(n)为启发式函数，用来约束路径的走向，代表指定结点（当前结点）到终点的横向或者纵向代价值
• A*算法的优势：减少了采样栅格个数，增加了路径搜索速度，优化了路径走向

; 具体实现

（二）基于采样的路径规划

1.RRT

定义及适用场景

快速拓展随机树法

单源路径、避障问题

核心思想

情况二：路径穿过障碍

需要舍弃该条路径选取

• 规范路径趋势 赋予终点一定的概率被选作采样点，以规范路径使其趋向最优（尽管效果不佳）
• 动态步长优化 连接两点之间的步长可以根据当前所处位置而动态变化，使路径更优

; 具体实现

伪代码：

主体核心代码+注释：

def rrt_planning(self, start, goal, animation=True):
        start_time = time.time()
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.node_list = [self.start]
        path = None

        for i in range(self.max_iter):

            rnd = self.sample()

            n_ind = self.get_nearest_list_index(self.node_list, rnd)
            nearestNode = self.node_list[n_ind]

            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
            newNode = self.get_new_node(theta, n_ind, nearestNode)

            noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
            if noCollision:

                self.node_list.append(newNode)

                if animation:
                    self.draw_graph(newNode, path)

                if self.is_near_goal(newNode):

                    if self.check_segment_collision(newNode.x, newNode.y,
                                                    self.goal.x, self.goal.y):

                        lastIndex = len(self.node_list) - 1

                        path = self.get_final_course(lastIndex)
                        pathLen = self.get_path_len(path)
                        print("current path length: {}, It costs {} s".format(pathLen, time.time()-start_time))

                        if animation:
                            self.draw_graph(newNode, path)
                        return path

效果分析

• 搜索速度快
• 路径非最优
• 只能到达目标区域附近

; 2.RRT*

定义及适用场景

在RRT思想上寻找最优的路径

单源避障的最优路径问题

核心

• parent_node 当前结点加入一步检索，寻找其到附近结点的最短距离，从而更新父节点，以使路径达到最优

    def choose_parent(self, newNode, nearInds):
        if len(nearInds) == 0:
            return newNode

        dList = []

        for i in nearInds:
            dx = newNode.x - self.node_list[i].x
            dy = newNode.y - self.node_list[i].y
            d = math.hypot(dx, dy)
            theta = math.atan2(dy, dx)
            if self.check_collision(self.node_list[i], theta, d):

                dList.append(self.node_list[i].cost + d)
            else:

                dList.append(float('inf'))

        minCost = min(dList)
        minInd = nearInds[dList.index(minCost)]

        if minCost == float('inf'):
            print("min cost is inf")
            return newNode

        newNode.cost = minCost
        newNode.parent = minInd

        return newNode

    def find_near_nodes(self, newNode):
        n_node = len(self.node_list)

        r = 50.0 * math.sqrt((math.log(n_node) / n_node))

        d_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2
                  for node in self.node_list]

        near_inds = [d_list.index(i) for i in d_list if i  r ** 2]
        return near_inds

• rewire 寻找到最优父节点之后在一定圆范围内不断检索所有临近结点的组合方式，重新组合路径，使其达到最优的效果

    def rewire(self, newNode, nearInds):
        n_node = len(self.node_list)
        for i in nearInds:
            nearNode = self.node_list[i]
            d = math.sqrt((nearNode.x - newNode.x) ** 2
                          + (nearNode.y - newNode.y) ** 2)
            s_cost = newNode.cost + d

            if nearNode.cost > s_cost:
                theta = math.atan2(newNode.y - nearNode.y,
                                   newNode.x - nearNode.x)
                if self.check_collision(nearNode, theta, d):
                    nearNode.parent = n_node - 1
                    nearNode.cost = s_cost

具体实现

                nearInds = self.find_near_nodes(newNode)

                newNode = self.choose_parent(newNode, nearInds)

                self.node_list.append(newNode)
                self.rewire(newNode, nearInds)

效果实现

• 搜索速度快
• 路径生成趋于最优
• 无效的搜索过多
• 只能到达目标区域

; 3.informed RRT*

定义及适用场景

在RRT思想的基础上添加 椭圆采样约束*,优化路径、加快寻优速度

适用于单源快速避障的路径规划问题

核心

• 椭圆采样约束
• 二维空间下，以起始点所有直线为椭圆长轴构建椭圆范围；三维空间下，构建椭球体
• 确定Xcenter点，作为坐标系转化的偏置修正
• 计算起始点的起始距离作为Cmin
• 每次采样前，计算当前路径的代价长度作为Cbest；计算旋转矩阵Kabsch算法求解旋转矩阵
• 每次采样时，以单位圆的形式采样获取坐标Xball；计算r矩阵、C旋转矩阵
• 每次采样后，以公式 r C Xball+Xcenter 转化为椭圆坐标系下的坐标 重点：不断缩小椭圆的范围 *具体实现：Cbest是在迭代中不断更新变化的，设置r矩阵第一个元素为Cbest的一半，第二个元素是Cbest与起始点长度的平方和的开方的一半，因此椭圆范围会不断更新变化（缩小）

• 旋转矩阵 计算旋转矩阵Kabsch算法求解旋转矩阵

; 具体实现

    def informed_sample(self, cMax, cMin, xCenter, C):
        if cMax < float('inf'):

            r = [cMax / 2.0,
                 math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,
                 math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]
            L = np.diag(r)

            xBall = self.sample_unit_ball()

            rnd = np.dot(np.dot(C, L), xBall) + xCenter
            rnd = [rnd[(0, 0)], rnd[(1, 0)]]
        else:

            rnd = self.sample()

        return rnd

效果实现

• 渐进寻优速度加快
• 路径趋向最优的效果极佳
• 减少了无效的搜索

; 4.PRM

定义

以随机采样的形式采点，当代价

Original: https://blog.csdn.net/m0_51748753/article/details/122761884
Author: 我心里你最美
Title: 路径规划与优化学习系列（一）—路径规划算法