CN110285802B - 快速扩展随机树路径光滑方法 - Google Patents

快速扩展随机树路径光滑方法 Download PDF

Info

Publication number
CN110285802B
CN110285802B CN201910505286.1A CN201910505286A CN110285802B CN 110285802 B CN110285802 B CN 110285802B CN 201910505286 A CN201910505286 A CN 201910505286A CN 110285802 B CN110285802 B CN 110285802B
Authority
CN
China
Prior art keywords
node
path
new
random tree
random
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910505286.1A
Other languages
English (en)
Other versions
CN110285802A (zh
Inventor
姜媛媛
陶德俊
刘延彬
时美乐
余泳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Anhui University of Science and Technology
Original Assignee
Anhui University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Anhui University of Science and Technology filed Critical Anhui University of Science and Technology
Priority to CN201910505286.1A priority Critical patent/CN110285802B/zh
Publication of CN110285802A publication Critical patent/CN110285802A/zh
Application granted granted Critical
Publication of CN110285802B publication Critical patent/CN110285802B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种快速扩展随机树路径光滑方法。本发明方法是基于Douglas‑Peucker算法及B样条函数的路径光滑方法。首先，利用Douglas‑Peucker算法从快速扩展随机树算法产生的路径节点中提取若干节点作为关键路标；然后，采用B样条函数拟合关键路标，得到一条曲率连续的光滑路径，从而实现规划路径的光滑化，解决了快速扩展随机树算法产生的路径存在过多的冗余点，路径转折点较多的问题。

Description

快速扩展随机树路径光滑方法
技术领域
本发明涉及智能机器人路径规划领域，具体为一种快速扩展随机树路径光滑方法。
背景技术
自主移动机器人路径规划是指在给定起点与终点后，按照一定的性能指标，在机器人所处环境中找出一条可以避开障碍物的最优或者次优路径，是机器人自主导航中的关键技术环节。传统的路径规划算法有可视图法、Voronoi图法、Delaunay三角剖分法等，但它们通用性欠佳，算法受限于使用环境；为了克服传统算法的缺点，研究者们提出一些智能优化算法，例如：蚁群算法、遗传算法等，由于收敛速度、实时性等方面的缺点，智能优化算法的应用并不广泛。
传统的规划算法得到的路径通常是由多个线段组成的线段序列，路径为非光滑曲线，当机器人运动到线段交点时，须停下并调整运动方向至合适的角度，才能继续前进，进而造成机器人运动的不连续性，而光滑的路径可以使机器人连续地运动。因此，路径光滑是机器人路径规划过程一个必要且非常重要的环节。
快速扩展随机树(Rapidly-exploring Random Tree,RRT)算法，通过随机采样产生若干采样点，对状态空间中的采样点进行碰撞检测，从而避免了对空间的建模。因计算速度快，效率高，不需要对环境进行建模，能够解决多自由度机器人在复杂环境下和动态环境中的路径规划问题，RRT算法得到了广泛应用。
针对传统RRT算法产生的路径可能包含有棱角，路径转折较多，存在绕路的现象，本发明提出了一种简便而有效的基于Douglas-Peucke(D-P)算法和B样条函数的路径光滑算法，使得路径长度明显缩短，拐点明显减少，路径更加光滑。
发明内容
本发明所解决的技术问题在于提供一种快速扩展随机树路径光滑方法，在若干路径节点中提取出关键路标，去除多余的冗余节点，缩短路径长度，减少拐弯次数，使路径更光滑，更容易应用于机器人路径规划当中。
本发明所解决的技术问题采用以下技术方案来实现：
一种快速扩展随机树路径光滑方法，具体步骤为：
1)读取给定的环境地图，对地图矩阵进行二值化转换，生成值为0和1的二维整形数组，0代表可通过点，1代表障碍物点；
2)给定机器人的起始节点qstart与目标节点qgoal，判断给定的起始节点qstart和目标节点qgoal的坐标值在二维整形数组地图中的值是不是有1，若都不为1，则进行步骤3；若有1，则重新输入起始节点或目标节点；
3)以起始节点qstart作为快速扩展随机树(the Rapidly-exploring Random Tree，RRT)生成树的根节点，建立随机树T1，初始化时随机树T1只包含一个根节点qstart；
4)在转换地图中的起始节点qstart附近基于概率采样随机选择一个采样点qrand；
5)找出一个离qrand距离最近的一个邻近节点qnear，生成沿新节点qnear和随机节点qrand方向上，离邻近节点qnear距离为给定距离阈值r的新节点qnew；
6)若新节点qnew与障碍物发生碰撞，则树枝放弃本次生长，如果未发生碰撞，则将qnew加入随机树T1中，生成一个树枝；
7)之后重复步骤4、5、6，再选择一个离qnew距离最近的采样点，并选择邻近节点qnear和新节点qnew，直到目标节点等于新的节点(qgoal＝qnew)或者新的节点qnew和邻近节点qnear将目标节点qgoal包围。以生成的树枝为载体，连接起始节点与目标节点中的随机树T1上的各个节点，得到一条合法的无碰撞路径；
8)根据设定的Douglas-Peucker距离阈值ε，对RRT生成的路径各节点，利用Douglas-Peucker算法，提取出若干关键路标；
9)对提取出来的关键路标，利用B样条函数进行拟合，从而得到一条曲率连续的光滑路径。
所述步骤4)中，基于概率的采样方法为：
为了加快随机树到达目标点的速度，在随机树每次的生长过程中，根据随机概率来决定qrand是目标点还是随机点。在Sample函数中设定参数P，且0<P<1，每次得到一个0到1.0的随机值s，当0<s<P的时候，随机树朝目标节点生长；当P<s<1.0时，随机树朝一个随机方向生长，以加快随机树到达目标节点的速度。
所述步骤8)中，利用Douglas-Peucker算法提取关键路标的步骤如下：
(1)在路径首尾两点A，B之间连接一条直线段AB；
(2)得到路径上离该直线段AB距离最大的点C，计算其与线段AB的距离d；
(3)比较距离d与预先给定的距离阈值ε的大小，若d<ε，则把该直线段作为路径的近似；
(4)若距离d>ε，则用C将路径分为两段AC和BC，C即为关键路标，并分别再对两段路径AC和BC进行(1)～(3)处理；
(5)所有路径都处理完毕后，得到的分割点即为关键路标，依次连接各个关键路标形成的折线，即可以作为路径的近似。
所述步骤9)中，B样条函数拟合方法为：
采用准均匀B样条曲线对Douglas-Peucker算法所得到的近似路径进行光滑处理，Douglas-Peucker算法提取的关键路标即为样条曲线的控制顶点，曲线次数k＝2，节点矢量中两端节点具有重复度k+1，所有的内节点均匀分布，具有重复度1，从而生成曲率连续的光滑路径。
本发明与现有技术相比的优点在于：
1)本发明提出的算法为了加快随机树到达目标点的速度，在随机树每次的生长过程中，根据随机概率来决定qrand是目标点还是随机点。
2)本发明提出的算法充分利用Douglas-Peucker算法与B样条曲线在优化路径方面的优势，极大程度上提升了路径质量。D-P算法剔除了过多的冗余点，从若干路径节点中提取出了关键路标，解决了明显的绕路现象与拐点过多的问题，减少了路径的转折次数，路径长度明显缩短，经过B样条函数优化的路径，与原始路径相比，光滑度显著提升。
3)本发明提出的算法通用性很强，简单地图和复杂地图都能够对路径进行优化，效果明显。
附图说明
图1为本发明算法流程图；
图2为传统快速扩展随机树算法效果图：(a)搜索树生成，(b)连通路径；
图3为本发明在简单地图的效果图；
图4为本发明在复杂地图的效果图。
具体实施方式
结合附图，本发明具体实施步骤详细说明如下：
1)给定机器人所处环境的地图，读取环境地图，对地图矩阵进行二值化转换，生成值为0和1的二维整形数组，0代表可通过点，1代表障碍物点。
2)给定机器人的起始节点qstart与目标节点qgoal，判断给定的起始节点qstart和目标节点qgoal的坐标值在二维整形数组地图中的值是不是有1，若都不为1，则进行步骤3)；若有1，则重新输入起始节点或目标节点；
3)以起始节点qstart作为快速扩展随机树生成树的根节点，建立随机树T1；
4)在地图中qstart附近基于概率采样随机选择一个采样点qrand，在随机树每次的生长过程中，根据随机概率来决定qrand是目标点还是随机点。在Sample函数中设定参数P，且0<P<1，每次得到一个0到1.0的随机值s，当0<s<P的时候，随机树朝目标节点生长；当P<s<1.0时，随机树朝一个随机方向生长；
5)找出一个离随机节点qrand距离最近的一个邻近节点qnear，生成沿邻近节点qnear和随机节点qrand方向上，离邻近节点qnear距离为给定阈值r的新节点qnew；
6)若新节点qnew与障碍物发生碰撞，则放弃本次生长，如果未发生碰撞，则将qnew加入随机树T1中，生成一个树枝；
7)之后再选择一个离qnew距离最近的一个节点qnear，重复步骤4、5、6，直到目标节点等于新的节点(qgoal＝qnew)或者新的节点qnew和邻近节点qnear将目标节点qgoal包围。以生成的树枝为载体，连接起始节点与目标节点中的随机树T1上的各个节点，得到一条合法的无碰撞路径；
8)在路径首尾两点A，B之间连接一条直线段AB；
9)得到路径上离该直线段距离最大的点C，计算其与线段AB的距离d；
10)比较该距离d与预先给定的阈值ε的大小，若d<ε，则把该直线段作为路径的近似；
11)若距离d>ε，则用C将路径分为两段AC和BC，C即为关键路标，并分别再对两段路径AC和BC进行(8)～(10)处理；
12)所有路径都处理完毕后，得到的分割点即为关键路标，依次连接各个关键路标形成的折线，即可以作为路径的近似。
13)采用准均匀B样条曲线对Douglas-Peucker算法所得到的近似路径进行光滑处理，Douglas-Peucker算法提取的关键路标即为样条曲线的控制顶点，曲线次数k＝2，节点矢量中两端节点具有重复度k+1，所有的内节点均匀分布，具有重复度1，从而生成曲率连续的光滑路径。
图2为传统快速扩展随机树(RRT)算法的实验结果。图2(a)为RRT算法在起始节点与目标节点之间生成的完整搜索树，而图2(b)path为连通后的路径，整条路径曲曲折折，转折次数多，冗余节点非常多，存在明显的绕路现象，路径整体不够光滑，而机器人不停地拐弯的代价太高，所以整体路径不利于机器人的行走。
如图3和图4，为本发明改进后的路径光滑算法的实验结果，Original path为传统RRT算法得到的路径，D-P path为在Original path的基础上经D-P算法得到的路径，DP-Bspline path为本发明方法得到的路径。简单地图图3中D-P算法的阈值ε设置为25，从图3中的D-P path可以看出，D-P算法从若干原始路径节点中提取出了6个关键路标，经过优化的路径质量与原先的路径质量相比，提高了许多，剔除了多余的冗余节点，解决了绕路现象，拐点数由原先的20个减少到4个，光滑度有了较大的提升。图3中DP-B spline path，是对D-P算法处理后的路径，基于关键路标，继续利用准均匀B样条函数对路径进行优化而产生的。整条D-P path经过B样条函数处理后，路径质量继续提升，路径越来越光滑，为一条曲率连续的光滑曲线，与原始路径相比，长度大幅度缩短了约20.2％。复杂地图图4中，D-P算法的ε阈值为17，优化的路径转折次数由原先的20次减少为4次，去除了多余的拐点，路径长度缩短了约16.4％，整条路径更加光滑。由于光滑的路径更利于机器人的行走，使得快速扩展随机树路径光滑方法在实际路径规划应用中更进了一步。
总之，经过本发明方法对路径的光滑处理，冗余节点将减少，路径长度将缩短，路径光滑度明显提升。
提供以上实施例仅仅是为了描述本发明的目的，而非要限制本发明的范围。本发明的范围由所附权利要求限。不脱离本发明精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。

Claims (4)

1.一种快速扩展随机树路径光滑方法，其特征在于步骤如下：
1)读取给定的环境地图，对地图矩阵进行二值化转换，生成值为0和1的二维整形数组，0代表可通过点，1代表障碍物点；
2)给定机器人的起始节点qstart与目标节点qgoal，判断给定的起始节点qstart和目标节点qgoal的坐标值在二维整形数组地图中的值是不是有1，若都不为1，则进行步骤3)；若有1，则重新输入起始节点或目标节点；
3)以起始节点qstart作为快速扩展随机树(Rapidly-exploring Random Tree，RRT)生成树的根节点，建立随机树T1，初始化时随机树T1只包含一个根节点qstart；
4)在转换地图中的起始节点qstart附近基于概率采样随机选择一个采样点qrand；
5)找出一个离qrand距离最近的一个邻近节点qnear，生成沿qnear和qrand方向上，离qnear距离为给定距离阈值r的新节点qnew；
6)若新节点qnew与障碍物发生碰撞，则树枝放弃本次延伸，即生长，如果未发生碰撞，则将qnew加入随机树T1中，生成一个树枝；
7)之后重复步骤4)、5)、6)，再选择一个离qnew距离最近的采样点，并选择新的邻近节点qnear和新节点qnew，直到目标节点等于新的节点(qgoal＝qnew)或者新的节点qnew和邻近节点qnear将目标节点qgoal包围；以生成的树枝为载体，连接起始节点与目标节点中的随机树T1上的各个节点，得到一条合法的无碰撞路径；
8)根据设定的Douglas-Peucker距离阈值ε，对RRT生成的路径各节点，利用Douglas-Peucker算法，提取出若干关键路标；
9)对提取出来的关键路标，利用B样条函数进行拟合，从而得到一条曲率连续的光滑路径。
2.根据权利要求1所述的快速扩展随机树路径光滑方法，其特征在于：所述步骤4)中，基于概率采样的方法为：
在随机树每次的生长过程中，根据随机概率来决定qrand是目标点还是随机点；在Sample函数中设定参数P，且0<P<1，每次得到一个0到1.0的随机值s，当0<s<P的时候，随机树朝目标节点生长；当P<s<1.0时，随机树朝一个随机方向生长，以加快随机树到达目标节点的速度。
3.根据权利要求1所述的快速扩展随机树路径光滑方法，其特征在于：所述步骤8)中，利用Douglas-Peucker算法，提取关键路标的步骤为：
(3.1)在RRT生成的路径首尾两点A，B之间连接一条直线段AB；
(3.2)得到路径上离该直线段AB距离最大的点C，计算其与线段AB的距离d；
(3.3)比较距离d与预先给定的距离阈值ε的大小，若d<ε，则把该直线段作为路径的近似；
(3.4)若距离d>ε，则用C将路径分为两段AC和BC，C即为关键路标，并分别再对两段路径AC和BC进行(3.1)～(3.3)处理；
(3.5)所有路径都处理完毕后，得到的分割点即为关键路标，依次连接各个关键路标形成的折线，即可以作为路径的近似。
4.根据权利要求1所述的快速扩展随机树路径光滑方法，其特征在于：所述步骤9)中，B样条函数拟合方法为：
采用准均匀B样条函数对Douglas-Peucker算法所得到的近似路径进行光滑处理，Douglas-Peucker算法提取的关键路标即为样条曲线的控制顶点，曲线次数k＝2，节点矢量中两端节点具有重复度k+1，所有的内节点均匀分布，具有重复度1，从而生成曲率连续的光滑路径。
CN201910505286.1A 2019-06-11 2019-06-11 快速扩展随机树路径光滑方法 Active CN110285802B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910505286.1A CN110285802B (zh) 2019-06-11 2019-06-11 快速扩展随机树路径光滑方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910505286.1A CN110285802B (zh) 2019-06-11 2019-06-11 快速扩展随机树路径光滑方法

Publications (2)

Publication Number Publication Date
CN110285802A CN110285802A (zh) 2019-09-27
CN110285802B true CN110285802B (zh) 2022-09-16

Family

ID=68003863

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910505286.1A Active CN110285802B (zh) 2019-06-11 2019-06-11 快速扩展随机树路径光滑方法

Country Status (1)

Country Link
CN (1) CN110285802B (zh)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112799416B (zh) * 2019-10-24 2024-04-12 广州极飞科技股份有限公司 航线生成方法、设备和系统、无人作业系统及存储介质
US20230012987A1 (en) * 2020-01-03 2023-01-19 Nokia Technologies Oy Obtaining a navigation path
CN111504321B (zh) * 2020-04-10 2022-03-18 苏州大学 一种基于扩展维诺图特征的可复用搜索树方法
CN113539050B (zh) * 2020-04-20 2022-09-23 华为技术有限公司 数据处理方法、装置及设备
CN111723983B (zh) * 2020-06-12 2022-09-09 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种未知环境下无人机的时间参数化航路规划方法与系统
CN111879307A (zh) * 2020-06-22 2020-11-03 国网河北省电力有限公司信息通信分公司 一种基于车身参数及工程建设信息的车辆路径规划方法
CN112529243A (zh) * 2020-10-22 2021-03-19 西安超越申泰信息科技有限公司 一种基于路网图像的流向地图路径优化展示方法
CN113064426B (zh) * 2021-03-17 2022-03-15 安徽工程大学 一种改进双向快速搜索随机树算法的智能车路径规划方法
CN113084811B (zh) * 2021-04-12 2022-12-13 贵州大学 一种机械臂路径规划方法
CN113467476B (zh) * 2021-08-02 2023-04-28 福州大学 考虑转角约束的无碰撞检测快速随机树全局路径规划方法
CN113843789A (zh) * 2021-08-13 2021-12-28 南京蓝昊智能科技有限公司 一种用于减轻机械臂刚性冲击的方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108196536A (zh) * 2017-12-21 2018-06-22 同济大学 一种改进的无人车快速搜索随机树路径规划方法
CN108458717A (zh) * 2018-05-07 2018-08-28 西安电子科技大学 一种迭代的快速扩展随机树irrt的无人机路径规划方法
CN109668573A (zh) * 2019-01-04 2019-04-23 广东工业大学 一种改进rrt算法的车辆路径规划方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101667031B1 (ko) * 2009-11-02 2016-10-17 삼성전자 주식회사 로봇의 경로 계획 장치 및 그 방법

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108196536A (zh) * 2017-12-21 2018-06-22 同济大学 一种改进的无人车快速搜索随机树路径规划方法
CN108458717A (zh) * 2018-05-07 2018-08-28 西安电子科技大学 一种迭代的快速扩展随机树irrt的无人机路径规划方法
CN109668573A (zh) * 2019-01-04 2019-04-23 广东工业大学 一种改进rrt算法的车辆路径规划方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于MB-RRT~*的无人机多点航迹规划算法研究;陈晋音等;《计算机科学》;20180615;第98-103页 *

Also Published As

Publication number Publication date
CN110285802A (zh) 2019-09-27

Similar Documents

Publication Publication Date Title
CN110285802B (zh) 快速扩展随机树路径光滑方法
CN110962130B (zh) 基于目标偏向寻优的启发式rrt机械臂运动规划方法
CN110609547B (zh) 一种基于可视图引导的移动机器人规划方法
CN109579854B (zh) 基于快速扩展随机树的无人车避障方法
CN113064426B (zh) 一种改进双向快速搜索随机树算法的智能车路径规划方法
CN106679667B (zh) 面向多导航站接力导航的运动体路径规划方法
CN107883961A (zh) 一种基于Smooth‑RRT算法的水下机器人路径优化方法
CN111323037B (zh) 一种移动机器人新型骨架提取的Voronoi路径规划算法
CN112306067B (zh) 一种全局路径规划方法及系统
CN113359775B (zh) 一种动态变采样区域rrt无人车路径规划方法
CN105512769A (zh) 基于遗传规划的无人机航迹规划系统及方法
Yang Anytime synchronized-biased-greedy rapidly-exploring random tree path planning in two dimensional complex environments
CN114115362B (zh) 一种基于双向apf-rrt*算法的无人机航迹规划方法
CN111678523A (zh) 一种基于star算法优化的快速bi_rrt避障轨迹规划方法
CN113961004A (zh) 海盗区域船舶航线规划方法、系统、电子设备及存储介质
CN112799405B (zh) 基于动态障碍物环境下的无人船路径规划方法
CN114839968A (zh) 一种水面无人艇路径规划方法
WO2022007227A1 (zh) 一种自动泊车的方法和车辆
CN117109625B (zh) 一种基于改进prm算法的无人驾驶路径规划方法
CN112197783B (zh) 一种考虑车头指向的两阶段多抽样的rrt路径规划方法
CN113721622A (zh) 一种机器人路径规划方法
CN116852367A (zh) 一种基于改进RRTstar的六轴机械臂避障路径规划方法
CN115060281B (zh) 一种基于voronoi图的全局路径引导点生成规划方法
CN115933749A (zh) 一种面向设备目标驱动的变电站巡检航线生成方法
CN116652932A (zh) 一种基于改进式rrt*算法的机械臂自主避障方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant