Nothing Special   »   [go: up one dir, main page]

CN114415652B - 一种轮式机器人路径规划方法 - Google Patents

一种轮式机器人路径规划方法 Download PDF

Info

Publication number
CN114415652B
CN114415652B CN202111318627.8A CN202111318627A CN114415652B CN 114415652 B CN114415652 B CN 114415652B CN 202111318627 A CN202111318627 A CN 202111318627A CN 114415652 B CN114415652 B CN 114415652B
Authority
CN
China
Prior art keywords
cost
road
robot
axis
path planning
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
CN202111318627.8A
Other languages
English (en)
Other versions
CN114415652A (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.)
Nanjing Nanzi Information Technology Co ltd
Original Assignee
Nanjing Nanzi Information Technology Co ltd
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 Nanjing Nanzi Information Technology Co ltd filed Critical Nanjing Nanzi Information Technology Co ltd
Priority to CN202111318627.8A priority Critical patent/CN114415652B/zh
Publication of CN114415652A publication Critical patent/CN114415652A/zh
Application granted granted Critical
Publication of CN114415652B publication Critical patent/CN114415652B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process

Landscapes

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

Abstract

本发明属于机器人路径规划应用技术领域,具体公开了一种轮式机器人路径规划方法,包括以下步骤,步骤1、建立生成三维点云地图,步骤2、根据三维点云地图生成包含路面拥堵情况、障碍物尺寸、路面坡度信息的三维格栅地图。步骤3、计算路面每个坡度节点间的坡度代价。步骤4、实时计算路面的拥堵代价。步骤5、将路面坡度代价及实时路面拥堵代价加入D*算法的总代价,利用D*算法进行路径规划。本发明的一种轮式机器人路径规划方法的有益效果在于:1、算法运行时间短,能较快找到最优路径;2、实现了复杂环境下的路径智能规划,能有效应对有障碍物及陡坡路段。

Description

一种轮式机器人路径规划方法
技术领域
本发明属于机器人路径规划应用技术领域,具体涉及一种轮式机器人路径规划方法。
背景技术
随着人工智能、物联网、云计算等技术的发展,机器人的研发设计、建造技术也获得了较大的提高,机器人正被人们越来越多地应用在各个领域。
SLAM(simultaneous localization and mapping),即时定位与地图构建以及路径规划一直是机器人研究领域中的核心问题。由于现实情况机器人所处的环境复杂多变,机器人的自动寻址能力受环境影响较大,其自动导航寻址能力仍然不够理想。
因此,基于上述问题,本发明提供一种轮式机器人路径规划方法。
发明内容
发明目的:本发明的目的是提供一种轮式机器人路径规划方法,其目的旨在复杂环境下实现轮式机器人对障碍物、路面坡度、路面拥堵情况的识别,使其能找到一种较好地穿过障碍物、避免陡坡路面动力不足上不去以及避开拥堵路面的方法,使之能快速高效地到达目的地。
技术方案:本发明的一种轮式机器人路径规划方法,包括以下步骤,步骤1、建立生成三维点云地图,步骤2、根据三维点云地图生成包含路面拥堵情况、障碍物尺寸、路面坡度信息的三维格栅地图。步骤3、计算路面每个坡度节点间的坡度代价。步骤4、实时计算路面的拥堵代价。步骤5、将路面坡度代价及实时路面拥堵代价加入D*算法的总代价,利用D*算法进行路径规划。
本技术方案的,所述步骤3中根据机器人尺寸判断机器人是否可以穿过障碍物,更新三维格栅地图;对于上坡路段,根据机器人重量、路面摩擦系数、机器人最大动力生成D*路径搜索代价函数附加信息项A(n)的子项。
本技术方案的,所述步骤4中根据路面拥堵情况,生成D*路径搜索代价函数附加信息项A(n)的子项。
本技术方案的,所述步骤5中将代价函数附加信息项A(n)加入到D*路径搜索算法中的代价函数F(n)中,形成代价总函数F(n)=G(n)+H(n)+A(n);以代价总函数F(n)为依据,进行D*路径寻优。
本技术方案的,所述步骤5中增加D*路径搜索算法代价函数附加信息项A(n),使之变为:F(n)=G(n)+H(n)+A(n),其中G(n)为目标节点到任意节点n的代价,H(n)为从节点n到起始点的启发式代价,其中,附加信息A(n)包含有路面拥堵情况代价信息、障碍物尺寸代价信息、路面坡度代价信息,利用D*路径搜索算法对路径进行规划。
本技术方案的,所述步骤3中对于上坡路段,根据机器人重量计算上坡阻力f=μ(mgCosθ)+mgSinθ,其中g为比例系数,大小约为9.8N/kg,θ路面坡度;步骤4中对于地图中包含的拥堵信息,设置kH(n)为附加信息项A(n)的一个子项,k为路面的拥堵系数,路面拥堵系数k的范围为0≤k≤1,k=0为路面畅通,k=1为路面严重拥堵。
本技术方案的,θ为相邻节点与地平面所形成的坡度角,计算公式为:
Xa、Xb、Xc为A点的x轴、y轴、z轴坐标,Ya、Yb、Yc为B点的x轴、y轴、z轴坐标。
本技术方案的,附加信息项代价D为机器人所能输出的最大动力,将附加信息项A(n)代价加入D*总代价函数中,得到新的总代价函数F(n)=G(n)+H(n)+A(n)。
本技术方案的,若机器人动力系统不足以提供大于上坡阻力f的动力,则设置附加信息项A(n)的代价为无穷大,否则设置其中一子项为
本技术方案的,H(n)为欧拉距离,计算公式为:
与现有技术相比,本发明的一种轮式机器人路径规划方法的有益效果在于:1、算法运行时间短,可以较快找到最优路径;2、实现了复杂环境下的路径智能规划,可以有效应对有障碍物及陡坡路段。
附图说明
图1是本发明的一种轮式机器人路径规划方法的算法流程图。
具体实施方式
下面结合附图和具体实施例,进一步阐明本发明。
本发明的一种轮式机器人路径规划方法,包括以下步骤,步骤1、建立生成三维点云地图,步骤2、根据三维点云地图生成包含路面拥堵情况、障碍物尺寸、路面坡度信息的三维格栅地图。步骤3、计算路面每个坡度节点间的坡度代价。步骤4、实时计算路面的拥堵代价。步骤5、将路面坡度代价及实时路面拥堵代价加入D*算法的总代价,利用D*算法进行路径规划。
本发明的轮式机器人路径规划方法,步骤3中根据机器人尺寸判断机器人是否可以穿过障碍物,更新三维格栅地图;对于上坡路段,根据机器人重量、路面摩擦系数、机器人最大动力生成D*路径搜索代价函数附加信息项A(n)的子项。
本发明的轮式机器人路径规划方法,步骤4中根据路面拥堵情况,生成D*路径搜索代价函数附加信息项A(n)的子项。
本发明的轮式机器人路径规划方法,步骤5中将代价函数附加信息项A(n)加入到D*路径搜索算法中的代价函数F(n)中,形成代价总函数F(n)=G(n)+H(n)+A(n);以代价总函数F(n)为依据,进行D*路径寻优。
进一步的,步骤5中增加D*路径搜索算法代价函数附加信息项A(n),使之变为:F(n)=G(n)+H(n)+A(n),其中G(n)为目标节点到任意节点n的代价,H(n)为从节点n到起始点的启发式代价,其中,附加信息A(n)包含有路面拥堵情况代价信息、障碍物尺寸代价信息、路面坡度代价信息,利用D*路径搜索算法对路径进行规划。
进一步的,步骤3中对于上坡路段,根据机器人重量计算上坡阻力f=μ(mgCosθ)+mgSinθ,其中g为比例系数,大小约为9.8N/kg,θ路面坡度;步骤4中对于地图中包含的拥堵信息,设置kH(n)为附加信息项A(n)的一个子项,k为路面的拥堵系数,路面拥堵系数k的范围为0≤k≤1,k=0为路面畅通,k=1为路面严重拥堵。
本发明的轮式机器人路径规划方法,θ为相邻节点与地平面所形成的坡度角,计算公式为:
Xa、Xb、Xc为A点的x轴、y轴、z轴坐标,Ya、Yb、Yc为B点的x轴、y轴、z轴坐标。
本发明的轮式机器人路径规划方法,附加信息项代价 D为机器人所能输出的最大动力,将附加信息项A(n)代价加入D*总代价函数中,得到新的总代价函数F(n)=G(n)+H(n)+A(n)。
本发明的轮式机器人路径规划方法,若机器人动力系统不足以提供大于上坡阻力f的动力,则设置附加信息项A(n)的代价为无穷大,否则设置其中一子项为
本发明的轮式机器人路径规划方法,H(n)为欧拉距离,计算公式为:
如图1所示,首先建立三维格栅地图的时候需包含障碍物的尺寸大小,如桌子侧面的大小,以便于与机器人本身的尺寸进行比较,以此判断机器人是否可以穿过障碍物;其次,若机器人在移动的时候发现路径上有新的障碍物,需要重新扫描构建新的三维格栅地图;最后路面拥堵代价主要是应用于多台机器人同时工作的场景,以此来防止本机器人与其它机器人挤占路面的情况,路面的拥堵代价需要单独的进程进行实时计算。
实施例
一种机器人路径规划方法,包括如下步骤:
1)本实施例轮式机器人配置了激光雷达和深度相机,用于建立三维点云地图,建立三维点云地图的方法很多,可以采用RTAB-MAP等SLAM方法,本实施例采用深度相机,获取环境深度图,然后将环境深度图转为点云;
2)本实施例通过投影模型计算点云世界坐标系坐标,采用三维刚体变换原理得到,深度图像与点云坐标之间的转换关系如下:
z=d
其中x,y,z是相机坐标系下的三维坐标,u,v为图像中像素的位置(所在的行和列),cx和cy是相机光学在图像坐标系下的坐标,若相机没有畸变,cx和cy分别为w/2和H/2,其中W和H分别为图像的宽度和高度。fx和fy分别为相机在x轴和y轴的焦距。
3)将路面拥堵情况,障碍物尺寸信息加入三维格栅地图中,形成具有路面拥堵情况、障碍物尺寸信息、路面坡度信息的三维格栅地图。
4)建立一个优先队列(OpenList)以目标点为起始点,将目标点置于OpenList中;
5)将地图中路径点用State表示,每个State包含指向前一个State的指针(Backpointer);当前State的状态Tag,有New、Open、Closed三种状态,New表示该State未被放置于OpenList中,Open表示该State正放置于OpenList中,CLosed表示该State已经从OpenList中取出;
6)将目标点的所有邻近节点(障碍物及不可达的节点除外)也放入优先队列(OpenList)中,计算每个目标点的邻近点到目标点的代价及到机器人位置节点的估计代价,计算出OpenList表中到目标节点代价最小的节点;
7)从OpenList表中移出到目标节点估计代价最小的节点,将该节点的邻居节点放入OpenList表中;
8)重复步骤6、7,直到机器人所在的节点的State的状态为Closed,或者OPenList表为空(表示不存在到目标节点的路径)
9)如果机器人在移动的过程中检测到环境发生变化,进入步骤3重新开始搜索。
以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以作出若干改进,这些改进也应视为本发明的保护范围。

Claims (7)

1.一种轮式机器人路径规划方法,其特征在于:包括以下步骤,
步骤1、建立生成三维点云地图;
步骤2、根据三维点云地图生成包含路面拥堵情况、障碍物尺寸、路面坡度信息的三维格栅地图;
步骤3、计算路面每个坡度节点间的坡度代价;
步骤4、实时计算路面的拥堵代价;
步骤5、将路面坡度代价及实时路面拥堵代价加入D*算法的总代价,利用D*算法进行路径规划;
所述步骤3中对于上坡路段,根据机器人重量计算上坡阻力f=μ(mgCosθ)+mgSinθ,其中u为阻力系数,m为机器人的重量,g为比例系数,大小约为9.8N/kg,θ路面坡度;步骤4中对于地图中包含的拥堵信息,设置kH(n)为附加信息项A(n)的一个子项,k为路面的拥堵系数,路面拥堵系数k的范围为0≤k≤1,k=0为路面畅通,k=1为路面严重拥堵;所述步骤5中增加D*路径搜索算法代价函数附加信息项A(n),使之变为:F(n)=G(n)+H(n)+A(n),其中G(n)为目标节点到任意节点n的代价,H(n)为从节点n到起始点的启发式代价,其中附加信息A(n)包含有路面拥堵情况代价信息、障碍物尺寸代价信息、路面坡度代价信息,利用D*路径搜索算法对路径进行规划。
2.根据权利要求1所述的一种轮式机器人路径规划方法,其特征在于:所述步骤3中根据机器人尺寸判断机器人是否可以穿过障碍物,更新三维格栅地图;对于上坡路段,根据机器人重量、路面摩擦系数、机器人最大动力生成D*路径搜索代价函数附加信息项A(n)的子项。
3.根据权利要求1所述的一种轮式机器人路径规划方法,其特征在于:所述步骤4中根据路面拥堵情况,生成D*路径搜索代价函数附加信息项A(n)的子项。
4.根据权利要求1所述的一种轮式机器人路径规划方法,其特征在于:θ为相邻节点与地平面所形成的坡度角,计算公式为:
Xa、Ya、Za为A点的x轴、y轴、z轴坐标,Yb、Yb、Zb为B点的x轴、y轴、z轴坐标。
5.根据权利要求1所述的一种轮式机器人路径规划方法,其特征在于:附加信息项代价D为机器人所能输出的最大动力,将附加信息项A(n)代价加入D*总代价函数中,得到新的总代价函数F(n)=G(n)+H(n)+A(n)。
6.根据权利要求1所述的一种轮式机器人路径规划方法,其特征在于:若机器人动力系统不足以提供大于上坡阻力f的动力,则设置附加信息项A(n)的代价为无穷大,否则设置其中一子项为
7.根据权利要求5所述的一种轮式机器人路径规划方法,其特征在于:H(n)为欧拉距离,计算公式为:Xa、Ya、Za为A点的x轴、y轴、z轴坐标,Yb、Yb、Zb为B点的x轴、y轴、z轴坐标。
CN202111318627.8A 2021-11-09 2021-11-09 一种轮式机器人路径规划方法 Active CN114415652B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111318627.8A CN114415652B (zh) 2021-11-09 2021-11-09 一种轮式机器人路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111318627.8A CN114415652B (zh) 2021-11-09 2021-11-09 一种轮式机器人路径规划方法

Publications (2)

Publication Number Publication Date
CN114415652A CN114415652A (zh) 2022-04-29
CN114415652B true CN114415652B (zh) 2024-03-26

Family

ID=81265145

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111318627.8A Active CN114415652B (zh) 2021-11-09 2021-11-09 一种轮式机器人路径规划方法

Country Status (1)

Country Link
CN (1) CN114415652B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118464053B (zh) * 2024-07-09 2024-09-03 北京理工大学 一种考虑稳定性的自动驾驶路径规划方法、设备及产品

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107526360A (zh) * 2017-09-26 2017-12-29 河南科技学院 一种未知环境下排爆机器人多阶自主导航探测系统及方法
CN108088445A (zh) * 2016-11-22 2018-05-29 广州映博智能科技有限公司 基于八叉树表示的三维栅格地图路径规划系统及方法
CN109798909A (zh) * 2019-02-01 2019-05-24 安徽达特智能科技有限公司 一种全局路径规划的方法
CN111046846A (zh) * 2019-12-27 2020-04-21 长安大学 一种机器人前方障碍通过性判断的方法
CN111830986A (zh) * 2020-07-24 2020-10-27 北京理工大学 一种用于双足机器人的三维路径规划方法
CN112034836A (zh) * 2020-07-16 2020-12-04 北京信息科技大学 一种改进a*算法的移动机器人路径规划方法
CN112572416A (zh) * 2020-11-20 2021-03-30 芜湖格陆博智能科技有限公司 一种矿区无人驾驶车辆的泊车方法及系统
CN112650234A (zh) * 2020-12-16 2021-04-13 浙江大学 一种双足机器人的路径规划方法
CN113485346A (zh) * 2021-07-15 2021-10-08 上海交通大学 一种移动机器人在核事故复杂环境中的自主导航方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108088445A (zh) * 2016-11-22 2018-05-29 广州映博智能科技有限公司 基于八叉树表示的三维栅格地图路径规划系统及方法
CN107526360A (zh) * 2017-09-26 2017-12-29 河南科技学院 一种未知环境下排爆机器人多阶自主导航探测系统及方法
CN109798909A (zh) * 2019-02-01 2019-05-24 安徽达特智能科技有限公司 一种全局路径规划的方法
CN111046846A (zh) * 2019-12-27 2020-04-21 长安大学 一种机器人前方障碍通过性判断的方法
CN112034836A (zh) * 2020-07-16 2020-12-04 北京信息科技大学 一种改进a*算法的移动机器人路径规划方法
CN111830986A (zh) * 2020-07-24 2020-10-27 北京理工大学 一种用于双足机器人的三维路径规划方法
CN112572416A (zh) * 2020-11-20 2021-03-30 芜湖格陆博智能科技有限公司 一种矿区无人驾驶车辆的泊车方法及系统
CN112650234A (zh) * 2020-12-16 2021-04-13 浙江大学 一种双足机器人的路径规划方法
CN113485346A (zh) * 2021-07-15 2021-10-08 上海交通大学 一种移动机器人在核事故复杂环境中的自主导航方法

Also Published As

Publication number Publication date
CN114415652A (zh) 2022-04-29

Similar Documents

Publication Publication Date Title
CN111210518B (zh) 基于视觉融合地标的拓扑地图生成方法
CN106371445B (zh) 一种基于拓扑地图的无人车规划控制方法
Ziegler et al. Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios
CN112378408A (zh) 一种实现轮式移动机器人实时避障的路径规划方法
CN110531770A (zh) 一种基于改进的rrt路径规划方法和系统
WO2021237667A1 (zh) 一种适用于腿足机器人规划的稠密高度地图构建方法
CN109974739B (zh) 基于高精度地图的全局导航系统及导航信息生成方法
CN113821029A (zh) 一种路径规划方法、装置、设备及存储介质
Jaspers et al. Multi-modal local terrain maps from vision and lidar
CN115639823B (zh) 崎岖起伏地形下机器人地形感知与移动控制方法及系统
CN111307158B (zh) 一种auv三维航路规划方法
KR20220083975A (ko) 궤적 계획 방법, 장치, 기기, 저장 매체 및 프로그램 제품
CN114415652B (zh) 一种轮式机器人路径规划方法
Short et al. Abio-inspiredalgorithminimage-based pathplanning and localization using visual features and maps
CN110209171A (zh) 一种基于人工势场法的路径规划方法
CN115576333B (zh) 最优向避障策略
CN118068367A (zh) 一种融合优先探索和定时器机制的三维激光雷达导航方法
CN117970928A (zh) 基于ros系统多移动机器人协同的路径规划方法
CN114564048A (zh) 一种面向农用四旋翼无人机航迹规划改进方法
CN114509085A (zh) 一种结合栅格和拓扑地图的快速路径搜索方法
Jin Research on Path Planning of Airport VIP Service Robot Based on A* Algorithm and Artificial Potential Field Method
Wu Path planning techniques for autonomous vehicles
Peng A Global Path Planning Method for USV Based on Improved A_Star Algorithm
Hexia ANALYSIS ON PATH OPTIMIZATION OF AGRICULTURAL HANDLING ROBOTS BASED ON ANT COLONY-IMPROVED ARTIFICIAL POTENTIAL FIELD METHOD.
Lei et al. An Improved Multi-objective Path Planning Algorithm and its Application to Tennis Ball Collection Robots

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