路径规划与优化学习系列(一)---路径规划算法
路径规划与优化学习系列(一)—路径规划算法
前言
几个月来浑浑噩噩,人生这张地图实在太大了,顿时觉得人生之路障碍重重、迷茫不清,故此受人启发,一学路径规划之法,以解心头之困,以便找到寻找最优之人生路(结尾有人生之悟)
本文仅用于笔记和回顾,学习来源如下介绍
学习目标
- 入门路径规划思想
- 深入了解经典路径规划思想
- 深入体会各种经典算法的适用场景
- 深入理解经典算法的代码实现
- 泛读论文,探索算法改进点
学习来源
B站(IR艾若机器人)机器人路径规划、轨迹优化系列课程_哔哩哔哩_bilibili
中国知网、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#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点的indexnearestNode = 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开始的下标,需要-1lastIndex = 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 newNodedList = []#遍历每一个范围内的结点,判断障碍并计算距离for i in nearInds:dx = newNode.x - self.node_list[i].xdy = newNode.y - self.node_list[i].yd = 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 = minCostnewNode.parent = minIndreturn newNodedef 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) ** 2for node in self.node_list]#遍历距离列表,寻找 在范围内的点 在d_list列表中的下标indexnear_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#如果附近结点(可选父节点)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 - 1nearNode.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*思想的基础上添加椭圆采样约束,优化路径、加快寻优速度
适用于单源快速避障的路径规划问题
核心
椭圆采样约束
二维空间下,以起始点所有直线为椭圆长轴构建椭圆范围;三维空间下,构建椭球体
确定Xcenter点,作为坐标系转化的偏置修正
计算起始点的起始距离作为Cmin
每次采样前,计算当前路径的代价长度作为Cbest;计算旋转矩阵Kabsch算法求解旋转矩阵
每次采样时,以单位圆的形式采样获取坐标Xball;计算r矩阵、C旋转矩阵
每次采样后,以公式 rCXball+Xcenter 转化为椭圆坐标系下的坐标
重点:不断缩小椭圆的范围
具体实现:Cbest是在迭代中不断更新变化的,设置r矩阵第一个元素为Cbest的一半,第二个元素是Cbest与起始点长度的平方和的开方的一半,因此椭圆范围会不断更新变化(缩小)
旋转矩阵
计算旋转矩阵Kabsch算法求解旋转矩阵
具体实现
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) + xCenterrnd = [rnd[(0, 0)], rnd[(1, 0)]]else:#未生成椭圆rnd = self.sample()return rnd#返回采样点的坐标
效果实现
- 渐进寻优速度加快
- 路径趋向最优的效果极佳
- 减少了无效的搜索
4.PRM
定义
以随机采样的形式采点,当代价<阈值时,生成点与点之间的直线路线,最后在搭建的路线图中寻找最优路径
概率路线图构建
随机采样
采样点和距离<阈值,则生成路线连接
图上寻找最优路径
以代价最低为目标寻找最优路线即可
(三)基于启发式智能算法的路径规划
1.遗传算法
2.蚁群算法
由于实际应用中较少,且以往已经接触和实现过,故在此不做笔记
三、经典算法的代码实现
代码的实现参考了IR艾若机器人的算法思路,深入理解了算法的核心部分
由于算法应用于不同的问题有不同的逻辑,故后续需要在不同问题场景下或者ros下跑一下仿真
(未完待续…)
四、经典算法的适用场景总结
五、论文泛读—算法改进
(另写一篇文章,未完待续…)
后记
入门路径优化算法√
人生之悟
路径虽有规划之法,但人生路毕竟多变,还需不断提升应变能力才能达到更好的下一个起点(人生没有终点)
人生路如此,无人机路径也如此,飞行环境多变,故此需要学习和探索应对多变环境的方法,使其增强生命的硬度!!!!
路径规划与优化学习系列(一)---路径规划算法相关推荐
- python绝对路径的区别_python学习:绝对路径和相对路径
大牛们应该对路径都很了解了,这篇文章主要给像我这样的入门小白普及常识用的,啊哈 下面的路径介绍针对windows,其他平台的暂时不是很了解. 在编写的py文件中打开文件的时候经常见到下面其中路径的表达 ...
- python职业规划书_Python学习过后,职业规划怎样规划?
机器学习的快速兴起下,Python逐步成为其开发的主要编程语言,简单易用的特点使得Python不仅成为了机器学习和数据科学的开发语言,同时在数据爬取.Web开发等多个方面也发挥着重要的作用.那么在Py ...
- 强化学习系列之Policy Gradient算法
一. 背景 1.1 基础组成部分 强化学习里面包含三个部件:Actor,environment,reward function Actor : 表示角色,是能够被玩家控制的. Policy of Ac ...
- 认真学习系列:数据结构与算法——慕课网笔记
一 数据的基本概念 1.数据:所有能输入计算机并被识别的符号 数据元素:数据基本单位,由若干数据项组成(比如数据库里的一条数据) 数据项:最小数据单位(比如数据库里一条数据的每一列就是一个数据项) 数 ...
- FPGA实现深度学习系列之卷积神经网络算法描述
这里全部内容都是由这个网址转载过来的. https://tech.youmi.net/2016/07/163347168.html 解说: 关于算法的完成.需要看很多的文章和视频才能有更好的理解和领悟 ...
- DevOps系列之 —— 持续规划与设计(四)敏捷需求管理【用户故事 敏捷估算】
DevOps系列之 -- DevOps概览(一)软件产业和交付模式发展趋势 DevOps系列之 -- DevOps概览(二)新型软件技术及交付模式 DevOps系列之 -- DevOps概览(三)De ...
- [学习笔记] [机器学习] 8. 聚类算法(聚类算法:K-means、K-means++;聚类算法评估;特征降维:特征选择(Pearson相关系数、Spearman相关系数)、PCA主成分分析)
视频链接 数据集下载地址:无需下载 1. 聚类算法简介 学习目标: 掌握聚类算法实现过程 知道 K-means 算法原理 知道聚类算法中的评估模型 说明 K-means 的优缺点 了解聚类中的算法优化 ...
- 学习笔记之——路径规划
最近在做移动机器人路径规划相关的topic,打算对路径规划算法做一个调研,并写下这篇记录.本博文的大部分内容来源于网络的博客或者论文,相关的参考也会给出来.本博文仅作本人学习记录用. 目录 引言 什么 ...
- 有关路径规划入门的学习记录
路径规划入门学习小记 路径规划算法实现 基于搜索的路径规划--Searching based 算法基础 BFS Breadth-First Search A*算法 算法实现 A*实现 BFS实现 基于 ...
- 基于强化学习的智能机器人路径规划算法研究(附代码)
目录 一.摘要 二.路径规划技术的研究进展 1.研究现状 2.算法分类 2.1 全局路径规划算法 2.2 局部路径规划算法 三.本文采用的路径规划算法--强化学习 1. 概念 2. 与其他机器学习方式 ...
最新文章
- Xmanager7如何通过SSH连接远程服务器
- OpenvSwitch — 安装部署与基本操作
- 计算整数n的b进制展开式
- 13 款 JavaScript 模板引擎
- sqoop从mysql到hive问题
- 通过入侵JVM打印阵列
- CaseStudy-数据缓存出错
- [RK3399][Android7.1] 如何将PWM背光线性处理
- Origin下载速度慢怎么办
- 测试theano有无使用GPU
- 使用 pandoc 进行文档转换(markdown转pdf)
- Linuux-alsa-左右声道处理
- 认识网络号与子网划分
- elk之拼音插件可选参数
- VCC、VDD、VSS、VDDA、VSSA、VREF+等标识
- oracle输出数据,Oracle数据库输出输入
- linux下查看磁盘空间
- 领导问“有空吗”,老实人回“没空”被敲打,用万能话术进退自如
- 重理工疫情期间自动打卡JS实现(每日上报+体温上报)
- 如何解决七牛云图片链接失效问题?
热门文章
- s3c2440linux2.6mmc/sd驱动程序
- matlab希尔伯特变换,基于matlab的Hilbert变换详解
- 被称为“Google 最大黑科技”,开发谷歌大脑,这位 AI 掌门人到底有多牛?
- python制作烟花特效_过大年,用Python去绽放最绚丽的烟花
- 室内智能照明控制系统电路设计
- Delphi 10.4.2 CE 社区版支持 Android API-30,之二
- 单片机 架构 程序 经验总结_单片机学习心得体会_经验总结
- 实验一:38译码器的VHDL实现及原理图实现
- 数字逻辑实验-交通灯控制设计
- 锐捷Linux版的下载和使用(福大客户端)