编程语言
首页 > 编程语言> > 路径规划与优化学习系列（一）---路径规划算法

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

作者：互联网

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

前言

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

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

学习目标

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

学习来源

  1. B站（IR艾若机器人）机器人路径规划、轨迹优化系列课程_哔哩哔哩_bilibili

  2. 中国知网、SCI、ieee论文库

正文

一、路径规划思想概述

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

1.基于飞行区域
2.基于任务时间
3.基于能耗优化
4.基于任务分配

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

二、路径规划经典算法

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

1.Dijkstra
背景及适用场景
原理：贪心思想

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

栅格地图

在这里插入图片描述

具体实现

取自：IR艾若机器人

在这里插入图片描述

2.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
        #rrt主体思想
        for i in range(self.max_iter):
            #采样点
            rnd = self.sample()
            #在self.node_list中获取near点
            n_ind = self.get_nearest_list_index(self.node_list, rnd)#返回near点的index
            nearestNode = self.node_list[n_ind]

            # steer：搭建树枝方向，以theta体现
            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:#noCollision=1表示树枝没有穿过障碍,记录path;否则舍弃这个newNode，继续sample
                #把newNode添加到node_list中
                self.node_list.append(newNode)
                #图像化仿真：显示寻找的过程
                if animation:
                    self.draw_graph(newNode, path)
                #判断newNode是否靠近goal：返回1则是
                if self.is_near_goal(newNode):
                    #是否穿过障碍
                    if self.check_segment_collision(newNode.x, newNode.y,
                                                    self.goal.x, self.goal.y):
                        #从0开始的下标，需要-1
                        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思想上寻找最优的路径

单源避障的最优路径问题

核心

在这里插入图片描述

    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)#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)
        #范围圆的半径:随着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]
        #遍历距离列表，寻找 在范围内的点 在d_list列表中的下标index
        near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]
        return near_inds#返回范围内点的下标

在这里插入图片描述

    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
            #如果附近结点(可选父节点)cost大于新选结点的cost，则更新临近结点的父节点和距离
            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
具体实现
                #两个优化技巧使路径趋向最优
                #寻找一定范围内的临近点(newNode的可能父节点)
                nearInds = self.find_near_nodes(newNode)
                #重新选择父节点:newNode的坐标不变，但cost和parent变了，即路径走向改变
                newNode = self.choose_parent(newNode, nearInds)
                #树中添加新结点信息，并且重新连接
                self.node_list.append(newNode)
                self.rewire(newNode, nearInds)
效果实现

在这里插入图片描述

3.informed RRT*
定义及适用场景

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

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

核心

在这里插入图片描述

具体实现
    def informed_sample(self, cMax, cMin, xCenter, C):
        if cMax < float('inf'):
            # L矩阵第一个元素为长轴的一半，第二个元素是当前路线长度与起始点长度的平方和的开方的一半
            r = [cMax / 2.0,
                 math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,
                 math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]
            L = np.diag(r)#生成以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

定义

以随机采样的形式采点，当代价<阈值时，生成点与点之间的直线路线，最后在搭建的路线图中寻找最优路径

概率路线图构建
图上寻找最优路径

以代价最低为目标寻找最优路线即可

（三）基于启发式智能算法的路径规划

1.遗传算法
2.蚁群算法

由于实际应用中较少，且以往已经接触和实现过，故在此不做笔记

三、经典算法的代码实现

四、经典算法的适用场景总结

在这里插入图片描述

五、论文泛读—算法改进

（另写一篇文章，未完待续…）

后记

入门路径优化算法√

在这里插入图片描述

人生之悟

路径虽有规划之法，但人生路毕竟多变，还需不断提升应变能力才能达到更好的下一个起点（人生没有终点）

人生路如此，无人机路径也如此，飞行环境多变，故此需要学习和探索应对多变环境的方法，使其增强生命的硬度！！！！

标签：node,self,路径,list,---,算法,newNode,规划
来源： https://blog.csdn.net/m0_51748753/article/details/122761884