CN113124875A - 一种路径导航方法 - Google Patents

一种路径导航方法 Download PDF

Info

Publication number
CN113124875A
CN113124875A CN202110419195.3A CN202110419195A CN113124875A CN 113124875 A CN113124875 A CN 113124875A CN 202110419195 A CN202110419195 A CN 202110419195A CN 113124875 A CN113124875 A CN 113124875A
Authority
CN
China
Prior art keywords
node
path
redundant
position node
target path
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.)
Withdrawn
Application number
CN202110419195.3A
Other languages
English (en)
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.)
Beijing Qinglong Technology Co ltd
Original Assignee
Beijing Qinglong 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 Beijing Qinglong Technology Co ltd filed Critical Beijing Qinglong Technology Co ltd
Priority to CN202110419195.3A priority Critical patent/CN113124875A/zh
Publication of CN113124875A publication Critical patent/CN113124875A/zh
Withdrawn legal-status Critical Current

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
    • G01C21/20—Instruments for performing navigational calculations

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

本发明提供了一种路径导航方法，属于路径规划技术领域，本发明提供一种路径导航方法，首先获取建筑信息模型，并提取建筑信息模型中所包含的障碍物几何信息，同时利用障碍物几何信息建立路径导航地图；根据路径导航地图利用全局搜索代价函数来获取原始目标路径，并对原始目标路径进行冗余路径点和冗余转折点删除，得到去除冗余转折点的目标路径，最后将除冗余转折点的目标路径进行平滑处理得到最优目标路径。本发明通过对原始目标路径依次进行冗余路径点和冗余转折点删除处理和平滑处理，可以使本发明规划的目标路径转折角大大减少，路径更短、更平滑，使机器人能连续在目标路径上运动，提高了机器人的工作效率。

Description

一种路径导航方法
技术领域
本发明属于路径规划技术领域，更具体地说，是涉及一种路径导航方法。
背景技术
传统的移动机器人自主导航地图的建立主要运用SLAM(simultaneouslocalization and mapping，即时定位与地图构建)技术，即在机器人对周围环境部分已知甚至完全未知的状态下，根据自身传感器获取的相关信息，完成对当前环境地图的构建与自身定位。该过程需要事先操作机器人在未知环境下进行完整的巡视检测，由外部传感器充分采集环境信息来构建地图，数据计算量大。对于建筑机器人而言，其本体尺寸、施工环境空间均很大，而移动速度则较为缓慢，故该方法用于建筑机器人将耗时、耗能。
路径规划旨在已知障碍物环境的前提下，为机器人规划出一条从起点到终点的最优路径。目前，运用于该类问题的算法主要包括A*算法。A*算法是一种适用于地图环境已知的全局路径规划算法，它可以保证在提高搜索效率的同时找到一条最优路径。A*算法最优节点之间使用动态窗口法进行局部避障，但是由于未考虑其输出的控制参数对机器的影响，在实际控制机器人运动过程中会导致机器人运动不连续，降低了机器人工作效率。
发明内容
本发明的目的在于提供一种路径导航方法，旨在解决现有路径规划算法在对机器人路径进行规划时，导致机器人运动不连续，降低了机器人工作效率的问题。
为实现上述目的，本发明采用的技术方案是：一种路径导航方法，包括：
步骤1：获取建筑信息模型；
步骤2：将所述建筑信息模型转换为IFC格式文件；
步骤3：提取所述IFC格式文件中所包含的障碍物几何信息；
步骤4：根据所述障碍物几何信息建立路径导航地图；
步骤5：根据所述路径导航地图利用全局路径搜索代价函数获取原始目标路径；
步骤6：将所述原始目标路径中的冗余路径点删除，生成去除冗余路径点的目标路径；
步骤7：将所述去除冗余路径点的目标路径中的冗余转折点删除，生成去除冗余转折点的目标路径；
步骤8：对所述去除冗余转折点的目标路径进行平滑处理得到最优目标路径。
优选的，所述步骤4：根据所述障碍物几何信息建立路径导航地图，包括：
步骤4.1：建立二维栅格图，其中所述二维栅格图的行、列栅格数生成公式为：
Figure BDA0003027214870000021
其中，Nr为行栅格数，Nc为列栅格数，xmax为建筑信息模型在x轴上的最大值，ymax为建筑信息模型在y轴上的最大值，s为步长；
步骤4.2：将所述障碍物几何信息映射到所述二维栅格图中，生成所述路径导航地图。
优选的，所述步骤5：根据所述路径导航地图利用全局路径搜索代价函数获取原始目标路径，包括：
步骤5.1：获取路径导航地图上当前位置节点和终点位置节点；
步骤5.2：根据所述当前位置节点得到所述当前位置节点周围的待遍历节点；
步骤5.3：计算所有所述待遍历节点与所述终点位置节点的距离代价值，生成距离代价值集合；
步骤5.4：取距离代价值集合中的最小值所对应的待遍历节点作为小车的下一个当前位置节点；
步骤5.5：判断所述下一个当前位置节点是否与所述终点位置节点重合，生成第一判断结果；
步骤5.6：若所述第一判断结果为所述下一个当前位置节点与所述终点位置节点重合，则原始目标路径规划完成；
步骤5.7：若所述第一判断结果为所述下一个当前位置节点与所述终点位置节点不重合，则返回所述步骤5.2。
优选的，所述步骤5.3：计算所有所述待遍历节点与所述终点位置节点的距离代价值，生成距离代价值集合，包括：
采用公式f(n)＝g(n)+eh(n)(h(n)+h(p))计算所有所述待遍历节点与所述终点位置节点的距离代价值，生成距离代价值集合；
其中，f(n)为待遍历节点与终点位置节点的距离代价值，g(n)为起始位置节点到当前位置节点的实际路径代价，
Figure BDA0003027214870000031
h(n)为当前位置节点到终点位置节点的估计路径代价，nx为当前位置节点的横坐标，ny为当前位置节点的纵坐标，goalx为终点位置节点的横坐标，goaly为终点位置节点的纵坐标，h(p)为当前位置节点的父节点到终点位置节点的距离。
优选的，所述步骤6：将所述原始目标路径中的冗余路径点删除，生成去除冗余路径点的目标路径，包括：
步骤6.1：获取原始目标路径节点序列；所述原始目标路径节点序列为M＝{P0,PN,Pk|k＝1,2…m}，其中Pk为中间位置节点，P0为起点位置节点，PN为终点位置节点；
步骤6.2：判断所述中间位置节点与所述中间位置节点相邻的前节点Pk-1和后节点Pk+1是否在一条直线上，生成第二判断结果；
步骤6.3：若所述第二判断结果为所述中间位置节点与所述中间位置节点相邻的前节点Pk-1和后节点Pk+1在一条直线上，则所述中间位置节点为冗余路径点，将所述冗余路径点从所述原始目标路径节点序列中删除，生成去除冗余路径点的目标路径序列；
步骤6.4：若所述第二判断结果为所述中间位置节点与所述中间位置节点相邻的前节点Pk-1和后节点Pk+1不在一条直线上，则保留所述中间位置节点；
步骤6.5：根据所述去除冗余路径点的目标路径序列生成所述去除冗余路径点的目标路径。
优选的，所述步骤7：将所述去除冗余路径点的目标路径中的冗余转折点删除，生成去除冗余转折点的目标路径，包括：
步骤7.1：获取去除冗余路径点的目标路径序列；所述去除冗余路径点的目标路径序列M′＝{P′0,P′N,P′k|k＝1,2…m}，其中P′k为去除冗余路径点后的中间位置节点，P0′为去除冗余路径点后的起点位置节点，P′N为去除冗余路径点后的终点位置节点；
步骤7.2：判断所述去除冗余路径点后的中间位置节点与所述去除冗余路径点后的中间位置节点相邻的前节点P′k-1和后节点P′k+1的距离值是否均为一个栅格的间距，生成第三判断结果；
步骤7.3：若所述第三判断结果为所述去除冗余路径点后的中间位置节点与所述去除冗余路径点后的中间位置节点相邻的前节点P′k-1和后节点P′k+1的距离值均为一个栅格的间距，则所述去除冗余路径点后的中间位置节点为冗余转折点，将所述冗余转折点从所述去除冗余路径点的目标路径序列中删除，生成去除冗余转折点的目标路径序列；
步骤7.4：若所述第三判断结果为所述去除冗余路径点后的中间位置节点与所述去除冗余路径点后的中间位置节点相邻的前节点P′k-1和后节点P′k+1的距离值均大于一个栅格的间距，则保留所述去除冗余路径点后的中间位置节点；
步骤7.5：根据所述去除冗余转折点的目标路径序列生成所述去除冗余转折点的目标路径。
优选的，所述步骤8：对所述去除冗余转折点的目标路径进行平滑处理得到最优目标路径，包括：
步骤8.1：建立约束条件；
步骤8.2：根据所述约束条件采用平滑处理公式拟合所述去除冗余转折点的目标路径，生成最优目标路径；其中，所述平滑处理公式为：
s(t)＝At5+Bt4+Ct3+Dt2+Et+F,t∈[0,T]
其中，s(t)为当前位置节点到终点位置节点的距离，A为第一系数向量,B为第二系数向量,C为第三系数向量,D为第四系数向量,E为第五系数向量,F为第六系数向量。
优选的，所述约束条件包括：路程约束条件、速度约束条件、加速度约束条件和加速度变化率约束条件；
所述路程约束条件为：s(T)＝0；
所述速度约束条件为：v(t)＝5At4+4Bt3+3Ct2+2Dt+E＝W，其中W为常数；
所述加速度约束条件为：a(t)＝20At3+12Bt2+6Ct+2D＝0；
所述加速度变化率约束条件为：a′(t)＝60At2+24Bt+6C＝0。
本发明提供的一种路径导航方法的有益效果在于：与现有技术相比，本发明的一种路径导航方法，首先获取建筑信息模型，并提取建筑信息模型中所包含的障碍物几何信息，同时利用障碍物几何信息建立路径导航地图；根据路径导航地图利用全局搜索代价函数来获取原始目标路径，并对原始目标路径进行冗余路径点和冗余转折点删除，得到去除冗余转折点的目标路径，最后将除冗余转折点的目标路径进行平滑处理得到最优目标路径。本发明通过对原始目标路径依次进行冗余路径点和冗余转折点删除处理和平滑处理，可以使本发明规划的目标路径转折角大大减少，路径更短、更平滑，使机器人能连续在目标路径上运动，提高了机器人的工作效率。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种路径导航方法流程图；
图2为本发明实施例提供的路径导航地图；
图3为本发明实施例提供的原始目标路径示意；
图4为本发明实施例提供的去除冗余转折点的目标路径示意图。
具体实施方式
为了使本发明所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本发明的目的在于提供一种路径导航方法，旨在解决现有路径规划算法在对机器人路径进行规划时，导致机器人运动不连续，降低了机器人工作效率的问题。
请参阅图1，为实现上述目的，本发明采用的技术方案是：一种路径导航方法，包括：
S1：获取建筑信息模型；
建筑信息模型即BIM模型，BIM模型具有信息参数化、信息关联性、信息完备性、信息一致性的特点。可使用Autodesk Revit软件进行BIM建模，来获取建筑信息模型。
S2：将建筑信息模型转换为IFC格式文件；
S3：提取IFC格式文件中所包含的障碍物几何信息；
BIM模型里包含了建筑物的一切几何信息及语义信息，在BIM模型完成之后，将其转换为IFC(IndustryFoundation Classes)文件。由于IFC文件建筑信息模型中语句的映射关系，通过语句管理表中的ID列与属性管理表中的ID列、映射管理表中的ID列之间的主、外键关系进行了完整的解析和保存，从而大大提高了BIM数据库中查询建筑信息的效率，因此本发明选择在IFC文件中提取建筑物的几何信息及语义信息。
S4：根据障碍物几何信息建立路径导航地图；
请参阅图2，S4具体包括：
S4.1：建立二维栅格图，其中二维栅格图的行、列栅格数生成公式为：
Figure BDA0003027214870000071
其中，Nr为行栅格数，Nc为列栅格数，xmax为建筑信息模型在x轴上的最大值，ymax为建筑信息模型在y轴上的最大值，s为步长，即每个栅格代表的实际长度；
S4.2：将障碍物几何信息映射到二维栅格图中，生成路径导航地图。
本发明在将障碍物几何信息映射到二维栅格图时，对于任意障碍物Z，如果Z占据但未完全占满某一栅格图时，将障碍物Z未完全占满某一栅格图进行扩张、补齐该栅格图，并将补齐后的栅格图也视为具有障碍物Z的区域。
S5：根据路径导航地图利用全局路径搜索代价函数获取原始目标路径；
请参阅图3，S5具体包括：
S5.1：获取路径导航地图上当前位置节点和终点位置节点；
在本发明中第一个当前位置节点即为起始位置起点。
S5.2：根据当前位置节点得到当前位置节点周围的待遍历节点；
在栅格图中，与当前位置节点相邻的节点即为待遍历节点。
S5.3：计算所有待遍历节点与终点位置节点的距离代价值，生成距离代价值集合；
S5.3具体包括：
采用公式f(n)＝g(n)+eh(n)(h(n)+h(p))计算所有待遍历节点与终点位置节点的距离代价值，生成距离代价值集合；
其中，f(n)为待遍历节点与终点位置节点的距离代价值，g(n)为起始位置节点到当前位置节点的实际路径代价，
Figure BDA0003027214870000081
h(n)为当前位置节点到终点位置节点的估计路径代价，nx为当前位置节点的横坐标，ny为当前位置节点的纵坐标，goalx为终点位置节点的横坐标，goaly为终点位置节点的纵坐标，h(p)为当前位置节点的父节点到终点位置节点的距离。
本发明考虑到当前节点到目标节点的估计代价占实际代价的比重与当前结点的位置有关，当前节点到目标节点的估计路径代价为最小路径代价，小于实际路径代价。当节点离目标节点较远时，估计值远小于实际值，所以应该使估计值权重大一些；当节点逐渐靠近目标节点时，估计值逐渐逼近实际值，估计值逐渐逼近实际值，估计值权重应该随之降低接近于1；当节点到目标节点之间没有障碍物时，估计值等于实际值，权值应该是1。对此，本发明使用指数衰减的方式进行加权构造全局路径搜索代价函数，当h(n)较大时，权重大，这样使结点迅速向目标点行进，当h(n)较小时，权重变小。目标点附近时，权重接近1，可保证目标点可达且距离最近。
节点A经过代价函数选出节点B为下一个节点，则节点A成为节点B父节点，节点B为节点A的子节点。在本发明中上一个当前位置节点即为当前位置节点的父节点。
S5.4：取距离代价值集合中的最小值所对应的待遍历节点作为小车的下一个当前位置节点；
S5.5：判断下一个当前位置节点是否与终点位置节点重合，生成第一判断结果；
S5.6：若第一判断结果为下一个当前位置节点与终点位置节点重合，则原始目标路径规划完成；
S5.7：若第一判断结果为下一个当前位置节点与终点位置节点不重合，则返回S5.2。
本发明虽然利用了全局路径搜索代价函数减少了运算量且可以更好地避开障碍，但是路径上增加了许多不必要的转折点，这对于机器人的路径跟随是不利的，会降低机器人行进速度，延长行程时间，因此，需要将冗余转折点删除，只保留必要的转折点。
S6：将原始目标路径中的冗余路径点删除，生成去除冗余路径点的目标路径；
S6具体包括：
S6.1：获取原始目标路径节点序列；原始目标路径节点序列为M＝{P0,PN,Pk|k＝1,2…m}，其中Pk为中间位置节点，P0为起点位置节点，PN为终点位置节点；
S6.2：判断中间位置节点与中间位置节点相邻的前节点Pk-1和后节点Pk+1是否在一条直线上，生成第二判断结果；
S6.3：若第二判断结果为中间位置节点与中间位置节点相邻的前节点Pk-1和后节点Pk+1在一条直线上，则中间位置节点为冗余路径点，将冗余路径点从原始目标路径节点序列中删除，生成去除冗余路径点的目标路径序列；
S6.4：若第二判断结果为中间位置节点与中间位置节点相邻的前节点Pk-1和后节点Pk+1不在一条直线上，则保留中间位置节点；
S6.5：根据去除冗余路径点的目标路径序列生成去除冗余路径点的目标路径。
下面对这一过程进行进一步说明，首先依次连接P0，P1，P2，如果这三个节点在同一直线上，则说明节点P1为多余节点，去除后不影响结果，此时将P1删除并更新M，之后继续连接P0，P2，P3，如果这三个节点不在同一直线上，则说明这三个节点中不存在多余节点，之后继续连接P4，P5，P6重复以上操作依次遍历所有路径节点，就可以删除所有冗余路径点。
S7：将去除冗余路径点的目标路径中的冗余转折点删除，生成去除冗余转折点的目标路径；
在本发明中，若当前路径节点与其前、后节点的距离均为一个栅格则视为冗余转折点，进行删除；若距离大于一个栅格则视为有效必要转折点保留。下面对这一过程进行进一步说明。
请参阅图4，S7具体包括：
S7.1：获取去除冗余路径点的目标路径序列；去除冗余路径点的目标路径序列M′＝{P′0,P′N,P′k|k＝1,2…m}，其中P′k为去除冗余路径点后的中间位置节点，P′0为去除冗余路径点后的起点位置节点，P′N为去除冗余路径点后的终点位置节点；
S7.2：判断去除冗余路径点后的中间位置节点与去除冗余路径点后的中间位置节点相邻的前节点P′k-1和后节点P′k+1的距离值是否均为一个栅格的间距，生成第三判断结果；
S7.3：若第三判断结果为去除冗余路径点后的中间位置节点与去除冗余路径点后的中间位置节点相邻的前节点P′k-1和后节点P′k+1的距离值均为一个栅格的间距，则去除冗余路径点后的中间位置节点为冗余转折点，将冗余转折点从去除冗余路径点的目标路径序列中删除，生成去除冗余转折点的目标路径序列；
S7.4：若第三判断结果为去除冗余路径点后的中间位置节点与去除冗余路径点后的中间位置节点相邻的前节点P′k-1和后节点P′k+1的距离值均大于一个栅格的间距，则保留去除冗余路径点后的中间位置节点；
S7.5：根据去除冗余转折点的目标路径序列生成去除冗余转折点的目标路径。
S8：对去除冗余转折点的目标路径进行平滑处理得到最优目标路径。
在实际工程中的运动控制分析可以得知，转向器对机器人施加转向力使机器人前进方向改变，当机器人的速度保持匀速时，最终机器人运动产生的路径是一条连续且光滑的曲线。用二维栅格图对BIM模型进行建模时，无论采用何种路径搜索算法，其规划得到的解都是一系列子节点组成的节点系列，再将各个子节点连接而得到一条路径，因此规划得到的路径都是有一定数量的线段构成的，所有不可避免地会有转折点和转折角出现，因此本发明对去除冗余转折点的目标路径进行平滑处理，可以输出一条适用于机器人行驶的全局光滑的曲线路径。下面对这一过程进行进一步描述。
S8具体包括：
S8.1：建立约束条件；约束条件包括：路程约束条件、速度约束条件、加速度约束条件和加速度变化率约束条件；
路程约束条件为：s(T)＝0；
速度约束条件为：v(t)＝5At4+4Bt3+3Ct2+2Dt+E＝W，其中W为常数；
W即为用户所设定的速度。
加速度约束条件为：a(t)＝20At3+12Bt2+6Ct+2D＝0；
加速度变化率约束条件为：a′(t)＝60At2+24Bt+6C＝0。
本发明中将机器人的行驶速度设定为匀速，并建立约束条件，可以很好地还原、拟合机器人在实际运行的路径，使机器人能连续在目标路径上运动，提高了机器人的工作效率。
S8.2：根据约束条件采用平滑处理公式拟合去除冗余转折点的目标路径，生成最优目标路径；其中，平滑处理公式为：
s(t)＝At5+Bt4+Ct3+Dt2+Et+F,t∈[0,T]
其中，s(t)为当前位置节点到终点位置节点的距离，A为第一系数向量,B为第二系数向量,C为第三系数向量,D为第四系数向量,E为第五系数向量,F为第六系数向量。
本发明提供的一种路径导航方法的有益效果在于：与现有技术相比，本发明的一种路径导航方法，首先获取建筑信息模型，并提取建筑信息模型中所包含的障碍物几何信息，同时利用障碍物几何信息建立路径导航地图；根据路径导航地图利用全局搜索代价函数来获取原始目标路径，并对原始目标路径进行冗余路径点和冗余转折点删除，得到去除冗余转折点的目标路径，最后将除冗余转折点的目标路径进行平滑处理得到最优目标路径。本发明通过对原始目标路径依次进行冗余路径点和冗余转折点删除处理和平滑处理，可以使本发明规划的目标路径转折角大大减少，路径更短、更平滑，使机器人能连续在目标路径上运动，提高了机器人的工作效率。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种路径导航方法，其特征在于，包括：
步骤1：获取建筑信息模型；
步骤2：将所述建筑信息模型转换为IFC格式文件；
步骤3：提取所述IFC格式文件中所包含的障碍物几何信息；
步骤4：根据所述障碍物几何信息建立路径导航地图；
步骤5：根据所述路径导航地图利用全局路径搜索代价函数获取原始目标路径；
步骤6：将所述原始目标路径中的冗余路径点删除，生成去除冗余路径点的目标路径；
步骤7：将所述去除冗余路径点的目标路径中的冗余转折点删除，生成去除冗余转折点的目标路径；
步骤8：对所述去除冗余转折点的目标路径进行平滑处理得到最优目标路径。
2.如权利要求1所述的一种路径导航方法，其特征在于，所述步骤4：根据所述障碍物几何信息建立路径导航地图，包括：
步骤4.1：建立二维栅格图，其中所述二维栅格图的行、列栅格数生成公式为：
Figure FDA0003027214860000011
其中，Nr为行栅格数，Nc为列栅格数，xmax为建筑信息模型在x轴上的最大值，ymax为建筑信息模型在y轴上的最大值，s为步长；
步骤4.2：将所述障碍物几何信息映射到所述二维栅格图中，生成所述路径导航地图。
3.如权利要求2所述的一种路径导航方法，其特征在于，所述步骤5：根据所述路径导航地图利用全局路径搜索代价函数获取原始目标路径，包括：
步骤5.1：获取路径导航地图上当前位置节点和终点位置节点；
步骤5.2：根据所述当前位置节点得到所述当前位置节点周围的待遍历节点；
步骤5.3：计算所有所述待遍历节点与所述终点位置节点的距离代价值，生成距离代价值集合；
步骤5.4：取距离代价值集合中的最小值所对应的待遍历节点作为小车的下一个当前位置节点；
步骤5.5：判断所述下一个当前位置节点是否与所述终点位置节点重合，生成第一判断结果；
步骤5.6：若所述第一判断结果为所述下一个当前位置节点与所述终点位置节点重合，则原始目标路径规划完成；
步骤5.7：若所述第一判断结果为所述下一个当前位置节点与所述终点位置节点不重合，则返回所述步骤5.2。
4.如权利要求3所述的一种路径导航方法，其特征在于，所述步骤5.3：计算所有所述待遍历节点与所述终点位置节点的距离代价值，生成距离代价值集合，包括：
采用公式f(n)＝g(n)+eh(n)(h(n)+h(p))计算所有所述待遍历节点与所述终点位置节点的距离代价值，生成距离代价值集合；
其中，f(n)为待遍历节点与终点位置节点的距离代价值，g(n)为起始位置节点到当前位置节点的实际路径代价，
Figure FDA0003027214860000021
h(n)为当前位置节点到终点位置节点的估计路径代价，nx为当前位置节点的横坐标，ny为当前位置节点的纵坐标，goalx为终点位置节点的横坐标，goaly为终点位置节点的纵坐标，h(p)为当前位置节点的父节点到终点位置节点的距离。
5.如权利要求4所述的一种路径导航方法，其特征在于，所述步骤6：将所述原始目标路径中的冗余路径点删除，生成去除冗余路径点的目标路径，包括：
步骤6.1：获取原始目标路径节点序列；所述原始目标路径节点序列为M＝{P0,PN,Pk|k＝1,2…m}，其中Pk为中间位置节点，P0为起点位置节点，PN为终点位置节点；
步骤6.2：判断所述中间位置节点与所述中间位置节点相邻的前节点Pk-1和后节点Pk+1是否在一条直线上，生成第二判断结果；
步骤6.3：若所述第二判断结果为所述中间位置节点与所述中间位置节点相邻的前节点Pk-1和后节点Pk+1在一条直线上，则所述中间位置节点为冗余路径点，将所述冗余路径点从所述原始目标路径节点序列中删除，生成去除冗余路径点的目标路径序列；
步骤6.4：若所述第二判断结果为所述中间位置节点与所述中间位置节点相邻的前节点Pk-1和后节点Pk+1不在一条直线上，则保留所述中间位置节点；
步骤6.5：根据所述去除冗余路径点的目标路径序列生成所述去除冗余路径点的目标路径。
6.如权利要求5所述的一种路径导航方法，其特征在于，所述步骤7：将所述去除冗余路径点的目标路径中的冗余转折点删除，生成去除冗余转折点的目标路径，包括：
步骤7.1：获取去除冗余路径点的目标路径序列；所述去除冗余路径点的目标路径序列M′＝{P′0,P′N,P′k|k＝1,2…m}，其中P′k为去除冗余路径点后的中间位置节点，P′0为去除冗余路径点后的起点位置节点，P′N为去除冗余路径点后的终点位置节点；
步骤7.2：判断所述去除冗余路径点后的中间位置节点与所述去除冗余路径点后的中间位置节点相邻的前节点P′k-1和后节点P′k+1的距离值是否均为一个栅格的间距，生成第三判断结果；
步骤7.3：若所述第三判断结果为所述去除冗余路径点后的中间位置节点与所述去除冗余路径点后的中间位置节点相邻的前节点P′k-1和后节点P′k+1的距离值均为一个栅格的间距，则所述去除冗余路径点后的中间位置节点为冗余转折点，将所述冗余转折点从所述去除冗余路径点的目标路径序列中删除，生成去除冗余转折点的目标路径序列；
步骤7.4：若所述第三判断结果为所述去除冗余路径点后的中间位置节点与所述去除冗余路径点后的中间位置节点相邻的前节点P′k-1和后节点P′k+1的距离值均大于一个栅格的间距，则保留所述去除冗余路径点后的中间位置节点；
步骤7.5：根据所述去除冗余转折点的目标路径序列生成所述去除冗余转折点的目标路径。
7.如权利要求6所述的一种路径导航方法，其特征在于，所述步骤8：对所述去除冗余转折点的目标路径进行平滑处理得到最优目标路径，包括：
步骤8.1：建立约束条件；
步骤8.2：根据所述约束条件采用平滑处理公式拟合所述去除冗余转折点的目标路径，生成最优目标路径；其中，所述平滑处理公式为：
s(t)＝At5+Bt4+Ct3+Dt2+Et+F,t∈[0,T]
其中，s(t)为当前位置节点到终点位置节点的距离，A为第一系数向量,B为第二系数向量,C为第三系数向量,D为第四系数向量,E为第五系数向量,F为第六系数向量。
8.如权利要求7所述的一种路径导航方法，其特征在于，所述约束条件包括：路程约束条件、速度约束条件、加速度约束条件和加速度变化率约束条件；
所述路程约束条件为：s(T)＝0；
所述速度约束条件为：v(t)＝5At4+4Bt3+3Ct2+2Dt+E＝W，其中W为常数；
所述加速度约束条件为：a(t)＝20At3+12Bt2+6Ct+2D＝0；
所述加速度变化率约束条件为：a′(t)＝60At2+24Bt+6C＝0。
CN202110419195.3A 2021-04-19 2021-04-19 一种路径导航方法 Withdrawn CN113124875A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110419195.3A CN113124875A (zh) 2021-04-19 2021-04-19 一种路径导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110419195.3A CN113124875A (zh) 2021-04-19 2021-04-19 一种路径导航方法

Publications (1)

Publication Number Publication Date
CN113124875A true CN113124875A (zh) 2021-07-16

Family

ID=76778109

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110419195.3A Withdrawn CN113124875A (zh) 2021-04-19 2021-04-19 一种路径导航方法

Country Status (1)

Country Link
CN (1) CN113124875A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113805584A (zh) * 2021-08-27 2021-12-17 上海擎朗智能科技有限公司 路径控制方法、机器人系统和计算机可读存储介质
CN114323045A (zh) * 2021-12-24 2022-04-12 浙江中控技术股份有限公司 路径规划方法及装置
CN115268435A (zh) * 2022-07-13 2022-11-01 东翼长启科技(重庆)有限公司 一种无人帆船的路径规划方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113805584A (zh) * 2021-08-27 2021-12-17 上海擎朗智能科技有限公司 路径控制方法、机器人系统和计算机可读存储介质
CN114323045A (zh) * 2021-12-24 2022-04-12 浙江中控技术股份有限公司 路径规划方法及装置
CN114323045B (zh) * 2021-12-24 2024-04-19 浙江中控技术股份有限公司 路径规划方法及装置
CN115268435A (zh) * 2022-07-13 2022-11-01 东翼长启科技(重庆)有限公司 一种无人帆船的路径规划方法

Similar Documents

Publication Publication Date Title
CN113124875A (zh) 一种路径导航方法
CN107168305B (zh) 路口场景下基于Bezier和VFH的无人车轨迹规划方法
CN112378408A (zh) 一种实现轮式移动机器人实时避障的路径规划方法
CN112526988B (zh) 一种自主移动机器人及其路径导航和路径规划方法、系统
Li et al. Mobile robot path planning based on improved genetic algorithm with A-star heuristic method
CN115755908B (zh) 一种基于JPS引导Hybrid A*的移动机器人路径规划方法
CN115826586B (zh) 一种融合全局算法和局部算法的路径规划方法及系统
CN113110455A (zh) 一种未知初始状态的多机器人协同探索方法、装置及系统
CN112857385A (zh) 一种基于非均匀栅格模型的快速无人车局部路径规划方法
CN113515111B (zh) 一种车辆避障路径规划方法及装置
CN114527761A (zh) 一种基于融合算法的智能汽车局部路径规划方法
Gu et al. Path planning for mobile robot in a 2.5‐dimensional grid‐based map
CN115328208A (zh) 一种实现全局动态路径规划的无人机路径规划方法
Vailland et al. Cubic bézier local path planner for non-holonomic feasible and comfortable path generation
CN115016458A (zh) 一种基于rrt算法的地洞勘探机器人路径规划方法
CN113721622A (zh) 一种机器人路径规划方法
Wang et al. Research on path planning of mobile robot based on improved jump point search algorithm
CN117968714A (zh) 一种面向复杂崎岖地形集群式地图的无人车辆导航路径规划系统及方法
CN111857148A (zh) 一种非结构化道路车辆路径规划方法
Lim et al. Safe Trajectory Path Planning Algorithm Based on Rrt* While Maintaining Moderate Margin from Obstacles
CN116795101A (zh) 一种融合改进a*与改进dwa算法的运动规划方法
Zhao et al. A study of the global topological map construction algorithm based on grid map representation for multirobot
CN116817913A (zh) 利用转弯惩罚因子和孪生路网改进的路径规划新方法
CN116009558A (zh) 一种结合运动学约束的移动机器人路径规划方法
CN116295485A (zh) 一种基于能量最优的无人车轨迹规划方法

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
WW01 Invention patent application withdrawn after publication
WW01 Invention patent application withdrawn after publication

Application publication date: 20210716