CN116817947B - 一种基于变步长机制的任意时路径规划方法 - Google Patents
一种基于变步长机制的任意时路径规划方法 Download PDFInfo
- Publication number
- CN116817947B CN116817947B CN202310624432.9A CN202310624432A CN116817947B CN 116817947 B CN116817947 B CN 116817947B CN 202310624432 A CN202310624432 A CN 202310624432A CN 116817947 B CN116817947 B CN 116817947B
- Authority
- CN
- China
- Prior art keywords
- node
- path
- current
- value
- evaluation function
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 37
- 230000007246 mechanism Effects 0.000 title claims abstract description 27
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 34
- 238000011156 evaluation Methods 0.000 claims description 35
- 238000001514 detection method Methods 0.000 claims description 16
- 230000008569 process Effects 0.000 claims description 7
- 230000007613 environmental effect Effects 0.000 claims description 5
- 238000005312 nonlinear dynamic Methods 0.000 claims description 5
- 230000006870 function Effects 0.000 description 24
- 230000003247 decreasing effect Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 5
- 238000005070 sampling Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000010845 search algorithm Methods 0.000 description 3
- 238000006467 substitution reaction Methods 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 239000013598 vector Substances 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 230000000052 comparative effect Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开一种基于变步长机制的任意时路径规划方法。步骤包括:(1)获取环境信息并网格化,确定起点和终点位置;(2)判断当前搜索步长是否不小于最小步长且有剩余搜索时间,若是,则进行路径搜索;否则,输出最新路径P_current并结束规划;(3)根据引入变步长机制的算法进行路径搜索,给出路径P;(4)将路径结点(不含起始结点)的f值从小到大排序,更新约束值集合并将最大f值设置为当前路径目标点的g值;(5)减小当前最大搜索步长,并继续执行步骤(2)。本算法可以解决固定翼无人机路径规划的任意时问题,具有规划速度快、生成路径平滑的特点,并能够对规划路径质量和规划时间进行有效平衡。
Description
技术领域
本发明涉及任意时路径规划技术领域,具体涉及一种基于变步长机制的任意时路径规划方法,尤其是涉及一种适用于固定翼无人机的、基于变步长机制的任意时路径规划方法,该方法能够规划出满足固定翼无人机动力学约束的可行路径,在有效控制次优路径边界的同时,能够对路径质量和规划时间进行有效平衡。
背景技术
路径规划算法的目的是在规划空间中寻找从起始点到目标点的无碰撞可行路径。它们大致可以分为两大类:一种是基于搜索的路径规划算法,以A*算法为代表;另一种是基于采样的路径规划算法,以RRT(Rapidly-Exploring Random Trees快速搜索随机树)算法为代表。两者的主要区别在于对规划空间的要求。基于搜索的算法需要离散空间,如网格映射,而基于采样的算法可以使用连续空间。基于搜索的算法具有完备性和最优性的优点,但在高维环境中,由于空间的复杂性,搜索效率较低。基于采样的算法适用于高维空间,但通常是非最优或渐近最优,且搜索路径具有随机性。尽管近年来基于采样的路径规划研究较多,但基于搜索的方法使用定义良好的采样将规划空间离散化,将路径规划问题转化为图搜索,比原来的连续问题简单得多,所以被广泛应用于路径规划领域。
在真实世界的路径规划过程中,能够用于规划的时间往往受到不同应用场景的限制,特别是对于高速运动的载具。为了解决这类问题,任意时路径规划算法通常会快速找到一条可行路径,然后继续搜索优化可行路径,直到时间耗尽或找到最优路径。在路径规划算法的研究中,目前基于A*的任意时路径规划算法主要是通过在启发式函数前乘一个常系数(称为膨胀因子)来获得次优解,以提高解的搜索效率。通过执行具有一系列膨胀因子递减的A*搜索算法,可以找到一系列最终收敛到最优解的改进解。这类算法主要有ARA*(Anytime Repairing A*)、AD*(Anytime Dynamic A*)以及后续改进的iADA*(improvedAnytime Dynamic A*);它们可以在给定时间限制下生成一条次优路径,然后重新用以前执行的搜索结果来改进路径的最优性。然而,基于膨胀因子的任意时路径规划算法难以有效控制次优路径的生成及其边界,以至于不能根据应有场景对路径质量和规划时间进行有效平衡。当有较大的凹形障碍物存在时,较大的膨胀因子还会降低搜索效率,使得这类算法难以在实际环境中应用。
在固定翼无人机路径规划场景中,由于飞机有着较快的飞行速度且气动力与飞行速度具有耦合性,上述传统路径规划算法都难以保证规划路径的可行性。满足一定条件下的路径平滑性和规划时效性要求是路径可行性的基础。以往基于搜索的算法受限于栅格化的地图,规划路径由较多折线组成,难以保证路径的平滑性;而基于采样的算法每次规划具有随机性,又难以适应高速飞行的时效性要求。因此,在有限时间内规划出一条符合固定翼无人机动力学约束的可行路径仍是一个亟待解决的问题。
发明内容
本发明的目的在于提出一种基于变步长机制的任意时路径规划方法,该方法能够适用于高速飞行的固定翼无人机。相比采用递减膨胀因子的方法,采用变步长机制能够有效控制次优路径的界限,同时保证路径平滑性以满足无人机的动力学约束;再通过引入椭圆(高维空间中称为超椭球)约束,实现了对先前规划信息的利用,具有路径规划速度快、生成路径平滑的特点,而且能够对路径质量和规划时间进行有效平衡,适用于多种任务场景。
本发明包含如下步骤:
步骤S1:根据激光雷达等感知端传感器获取的环境信息建立网格地图,标记每个障碍物占据的栅格,并设定路径规划的起始点S、目标点G、用于搜索的时间、最大搜索步长Step_max(大于1的整数),将评价函数f(s)的最大约束值f_max初始化为无穷大;
步骤S2:引入变步长机制的Search_radiation_bound算法,判断最大搜索步长是否不小于最小步长(最小步长根据具体型号固定翼无人机的最小转弯半径进行设置)且有剩余搜索时间,若是,继续执行步骤S3;否则,输出当前路径P_current并结束路径规划;
步骤S3:根据步骤S2变步长机制的Search_radiation_bound算法进行路径搜索,获得路径P,该路径是由一系列网格结点即路径点组成;
步骤S4:根据步骤S3生成路径P的路径点集合分别计算其中各个结点(除了起始结点和目标结点)的评价函数值f(s),按照从小到大的顺序将其存储于约束值集合f_Basic_points,并将评价函数的最大约束值f_max设置为当前路径目标结点的路径代价(从起始结点到目标结点实际所花费的代价)g(G);
步骤S5:减小当前最大搜索步长Step_max,具体可以根据实际应用场景选择递减形式;返回步骤S2。
具体的评价函数f(s)值的计算公式为:f(s)=g(s)+h(s)
其中,s为当前结点,g(s)为当前结点的路径代价,即从起始结点至当前结点s的实际代价;h(s)为启发函数,表示当前结点s至路径目标点G的预估代价,可以根据实际应用场景使用欧式距离、曼哈顿距离等度量。
进一步的,所述步骤S2具体为:
在进行单次搜索过程中需要维护两个存储列表(OPEN列表、CLOSED列表)和两个存储集合(OPEN_again集合、f_Basic_points集合);OPEN列表中存储待扩展的结点,CLOSED列表中存储已经扩展过的结点,OPEN_again集合中存储需要进行变步长扩展的结点,f_Basic_points集合中存储本次搜索中用于更新评价函数约束值f_bound的f值。
S2.1将CLOSE列表和OPEN_again集合初始化为空,并将规划的起始结点放入OPEN列表;判断是否为第一次规划,若是,则将评价函数值约束值f_bound初始化为f_max,否则,将f_bound初始化为约束值集合f_Basic_points中最小的f值,并将该最小f值从f_Basic_points中删除;
S2.2判断OPEN列表与OPEN_again是否同时为空,若是,则输出路径规划失败,终止搜索;否则,执行步骤S2.3;
S2.3选取OPEN列表中评价函数值最小的结点作为当前结点s,然后将当前结点s从所述OPEN列表中删除并放入CLOSED列表;
S2.4判断当前结点s是否为目标结点,若是,则得到Step_max搜索步长下的规划路径P,结束步骤S2;否则,执行步骤S2.5;
S2.5沿着水平、垂直、及对角方向,依次对与当前结点s相距Step_max个栅格距离的八个邻居结点进行碰撞检测,对于没有发生碰撞的邻居结点s_n,若其路径代价g(s_n)小于该邻居结点s_n之前的路径代价g_old(s_n)且评价函数f(s_n)不大于评价函数约束值f_bound,则执行步骤2.6;对当前结点s的邻居结点遍历完毕后,执行步骤S2.7;
S2.6对结点s_n进行可行性检测,若其处于可达域中,则更新结点s_n的g值,将当前结点s设置为s_n的父结点,并将s_n结点放入OPEN列表;否则,继续遍历下一个邻居结点;
S2.7判断当前结点s与其邻居结点的碰撞次数是否不少于7次,或者是否存在邻居结点s_n的路径代价g(s_n)小于s_n之前的路径代价g_old(s_n)但是评价函数f(s_n)大于评价函数约束值f_bound,若是,则将当前结点s放入OPEN_again集合;
S2.8判断OPEN列表是否有新增结点,若否,且存在邻居结点s_n的路径代价g(s_n)小于s_n之前的路径代价g_old(s_n)但是评价函数f(s_n)大于评价函数约束值f_bound,则将f_bound更新为约束值集合f_Basic_points中最小的f值,并将该最小f值从f_Basic_points中删除;若约束值集合为空,则将f_bound更新为f_max;
S2.9判断OPEN列表是否为空,若是,则将当前结点s放入OPEN_again集合并对该集合中的所有结点进行变步长扩展,然后将OPEN_again集合置为空,返回步骤S2.2。
进一步的,所述步骤S2.6(可行性检测)具体为:
S2.6.1根据当前结点s的位置信息获取固定翼无人机的飞行状态;
S2.6.2结合网格地图的分辨率,将结点s_n的位置信息作为关键点进行追踪,通过解算固定翼无人机非线性动力学与控制模型获取固定翼无人机的飞行轨迹;
S2.6.3检查固定翼无人机的飞行轨迹是否经过有障碍物的栅格,若是,则结点s_n处于不可达域内,否则,结点s_n处于可达域内。
进一步的,所述步骤S2.9的变步长扩展具体为:
S2.9.1将OPEN_again集合中的结点依次取出,作为当前结点s_again;
S2.9.2对于当前结点s_again,沿着水平、垂直、及对角方向,在特定步长范围内以步长递增的形式逐个栅格结点进行碰撞检测,直到遇到障碍物或达到特定步长Step_max,将相距最远的无碰撞结点作为当前结点s_again的邻居结点;
S2.9.3判断当前结点s_again的邻居结点s_n_again的路径代价g(s_n_again)是否小于s_n_again结点之前的路径代价,若是,则进一步判断其评价函数f(s_n_again)是否不大于评价函数约束值f_bound,若是,则更新结点s_n_again的g值,将s_again结点设置为s_n_again的父结点,并将s_n_again结点放入OPEN列表。
与现有技术相比,本发明具有以下优点:
1、本发明在大多数情况下对距离当前结点特定步长下的邻居结点进行扩展,相比于只将紧邻的栅格作为邻居结点,能够极大减少OPEN列表的长度,减少运行内存占用,同时更快地找到可行路径;
2、本发明引入了变步长机制,当OPEN列表为空时,对在特定步长下不能进行有效扩展的结点进行变步长扩展,在确保搜索算法完备性的同时,可以更好地适应凹凸障碍物混合的各种环境;
3、本发明通过最大搜索步长递减的方法引入椭圆(在高维空间下是超椭球)约束,对先前规划信息进行利用,在规划时间允许的情况下保证找到最优路径,同时使本发明具有增量搜索算法的特点,从而进一步提高搜索效率;
4、本发明摆脱了传统算法的单步长搜索限制,将固定翼无人机的动力学约束以可达域的形式引入路径搜索过程中,保证了规划路径的可行性。
附图说明
为了更清楚地说明本发明实施过程中的技术细节,下面将技术实施过程中的流程、机制、原理以及仿真验证实验的结果等进行附图展示。显然,下面描述中的附图仅是本发明中的部分实施例。
图1为本发明提出的一种基于变步长机制的任意时路径规划方法流程图;
图2为本发明提出的某一特定步长搜索下变步长机制的实施流程图;
图3为本发明提出的椭圆约束示意图;
图4a、b、c为本发明提出的对角障碍物碰撞检测原理图;
图5为本发明提出的固定翼无人机在某一时刻状态下的可达域示意图;
图6为获取当前结点与下一结点之间飞行轨迹的流程图;
图7a、b、c为本发明提出的特定步长和变步长扩展原理示意图;
图8a、b、c、d、e为具体实施例中基于变步长机制的任意时路径规划方法和传统算法得到的路径结果图以及各个算法的运行时间柱状图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例,都属于本发明保护的范围。
参阅图1,图1是本发明提出的一种基于变步长机制适用于固定翼无人机的任意时路径规划方法流程图。根据激光雷达等感知端传感器获取的环境信息建立网格地图,对每个障碍物占据的栅格进行标记,并设置路径规划的起始点S、目标点G、用于搜索的时间、最大搜索步长,初始化环境信息。初始运行需要根据具体应用场景设定最大搜索步长和用于搜索的时间,f值约束设置为无穷大。通过步长控制迭代搜索,当步长小于最小步长(根据具体型号固定翼无人机的最小转弯半径来设置)或超过预设的搜索时间时,搜索将停止并返回最后生成的路径。迭代搜索包括三个核心:特定步长设置、特定步长下的路径搜索和f值约束设置;在f值约束的基础上,进行特定步长下的路径搜索,然后存储当前路径规划结果并更新步长。
具体步骤如下:
步骤S1:根据激光雷达等感知端传感器获取的环境信息建立网格地图,标记每个障碍物占据的栅格,并设定路径规划的起始点S、目标点G、用于搜索的时间t_max、最大搜索步长Step_max(大于1的整数),将评价函数f(s)的最大约束值f_max初始化为无穷大;
步骤S2:引入变步长机制的Search_radiation_bound算法,判断最大搜索步长是否不小于最小步长且有剩余搜索时间,若是,继续执行步骤S3;否则,输出当前路径P_current并结束路径规划;
最小步长设置的具体实现如下:
其中,Stepmin表示当前任务环境下的最小搜索步长,Res表示当前任务环境下网格地图的分辨率,Rmin表示具体型号固定翼无人机的最小转弯半径,ceil表示向上取整函数;
步骤S3:根据引入变步长机制的Search_radiaton_bound算法进行路径搜索,获得路径P={s_1,s_2,…,s_n};
步骤S4:根据步骤S3生成路径P的路径点集合分别计算其中各个结点(除去起始结点和目标结点)的评价函数值(f(s_2),f(s_3),…,f(s_n-1)),按照评价函数值从小到大的顺序将其存储于约束值集合f_Basic_points,根据当前路径目标点的g值g(G)设置f_max;
具体的评价函数f(si)值的计算公式为:f(si)=g(si)+h(si)
其中,g(si)为从起始结点经过中间结点到结点si的路径长度;h(si)为结点si到路径目标点G的预估路径长度;路径长度的计算均使用欧式距离。
步骤S5:减小当前最大搜索步长Step_max,在二维网格地图空间中选择递减形式为Step_max=Step_max/3(步长向下取整);返回步骤S2。
参阅图2,图2是本发明提出的某一特定步长搜索下变步长机制的实施流程图。与A*算法类似,本方法先将起始结点放入OPEN列表,然后进入搜索循环。从OPEN列表中取出的f值最小的结点被放到CLOSED列表中。对于某一特定步长下的搜索,如果到达目标结点则发布路径点集合并终止本次搜索,否则,检查它的邻居结点。与A*算法不同的是,被检查并最终添加到OPEN列表中的邻居结点与当前结点的距离相同但通常大于一个栅格距离(即搜索步长大于1)。除上述情况外,本方法考虑了另外一种特殊情况:在特定步长的搜索过程中,若障碍物的分布恰好位于OPEN列表中所有结点的所有八个方向上,OPEN列表此时将弹出所有结点。因此,对于在寻找邻居结点的过程中至少有7次碰撞的结点,将其存储在OPEN_again集合中以备再次寻找邻居结点。当OPEN列表为空时,最后弹出的结点也将被添加到OPEN_again集合,并对OPEN_again集合中的结点使用变步长搜索。该变步长机制通过自适应的方式调整获取当前结点的邻居结点的步长,可以获得所有无障碍的邻居结点,保证路径搜索的完备性。换言之,在通常情况下,以固定步长获取当前结点的邻居结点;当OPEN列表为空时,获取距离当前结点不同步长的邻居结点。因此,当起始结点和目标结点之间存在可行路径时,算法只在到达目标结点时终止。具体步骤如下:
S2.1将的CLOSE列表和OPEN_again集合初始化为空,并将规划的起始结点放入OPEN列表;参阅图3,图3为本发明提出的椭圆约束示意图,可对先前规划信息进行利用。如果是第一次规划路径,则将f值约束f_bound初始化f_max,否则,将其初始化为约束值集合f_Basic_points中最小的f值,并将该最小f值从f_Basic_points中删除;
椭圆约束的具体实现如下:
如图3所示,对于某一结点si,若f(si)<=f_bound,则表示结点si处于可扩展的栅格结点区域内;否则,则认为该结点不可扩展;
S2.2判断OPEN列表与OPEN_again是否同时为空,若是,则输出路径规划失败,终止搜索;否则,执行步骤S2.3;
S2.3选取OPEN列表中评价函数值最小的结点作为当前结点s,将当前结点s从所述OPEN列表中删除并放入CLOSED列表;
S2.4判断当前结点s是否为目标结点,若是,则得到Step_max搜索步长下的规划路径P,结束步骤S2;否则,执行步骤S2.5;
S2.5参阅图4a、b、c,图4a、b、c为本发明提出的对角障碍物碰撞检测原理图。沿着水平、垂直,及对角方向,依次对与当前结点s相距Step_max个栅格距离的八个邻居结点进行碰撞检测,对于没有发生碰撞的邻居结点s_n,若其路径代价g(s_n)小于该邻居结点s_n之前的路径代价g_old(s_n)且f(s_n)不大于f_bound,则执行步骤2.6;对当前结点s的邻居结点遍历完毕后,执行步骤S2.7;
所述的碰撞检测具体实施方式如下:
在二维空间定义八个扩展向量,用来表征二维平面上的八个扩展方向。邻居结点的坐标由下式确定:
V={(1,0),(1,1),(0,1),(-1,1),(-1,0),(-1,-1),(0,-1),(1,-1)}
x(s_n)=x(s)+Step_max·v v∈V
其中,x(s)表示当前结点的坐标,x(s_n)表示s的邻居结点的坐标;v表示方向向量;
若邻居结点与当前结点的连线经过障碍物占据的栅格,则表示发生碰撞;若邻居结点与当前结点的连线为对角方向,则需要进行对角障碍物检测,若为图4a、b、c所示情况,则同样表示发生碰撞;
S2.6对结点s_n进行可行性检测,若其处于可达域中,则更新结点s_n的g值,将当前结点s设置为s_n的父结点,并将s_n结点放入OPEN列表,否则,继续遍历下一个邻居结点;
S2.7判断当前结点s与其邻居结点的碰撞次数是否不少于7次,或者是否存在邻居结点s_n的路径代价g(s_n)小于s_n之前的路径代价g_old(s_n)但是f(s_n)大于f_bound,若是,则将当前结点s放入OPEN_again集合;
S2.8判断OPEN列表是否有新增结点,若否,且存在邻居结点s_n的路径代价g(s_n)小于s_n之前的路径代价g_old(s_n)但是f(s_n)大于f_bound,则将f_bound更新为约束值集合f_Basic_points中最小的f值,并将该最小f值从f_Basic_points中删除;若约束值集合为空,则将f_bound更新为f_max;
S2.9判断OPEN列表是否为空,若是,则将当前结点s放入OPEN_again集合并对该集合中的所有结点进行变步长扩展,然后将OPEN_again集合置为空,返回步骤S2.2。
参阅图5,图5为本发明提出的固定翼无人机在某一时刻状态下的可达域示意图。进一步的,所述步骤S2.6(可行性检测)具体为:
S2.6.1根据当前结点s的位置信息获取固定翼无人机的飞行状态;
S2.6.2结合网格地图的分辨率,将结点s_n的位置信息作为关键点进行追踪,通过解算固定翼无人机非线性动力学与控制模型获取固定翼无人机的飞行轨迹;
所述的通过解算固定翼无人机非线性动力学与控制模型获取固定翼无人机飞行轨迹的流程具体如图6所示,将处于当前结点时的飞机状态信息作为当前时刻飞机的状态信息输入控制模型,同时将下一结点的位置信息也输入控制模型,得到飞机舵面控制量及油门杆量;然后将舵面控制量和油门杆量代入固定翼无人机的非线性动力学模型进行解算,得到下一时刻飞机状态信息;不断重复这一过程直至到达下一结点,得到当前结点到下一结点的飞行轨迹;
S2.6.3检查固定翼无人机的飞行轨迹是否经过有障碍物的栅格,若是,则结点s_n处于不可达域内,否则,结点s_n处于可达域内。
参阅图7a、b、c,图7a、b、c为本发明提出的特定步长和变步长扩展原理示意图。进一步的,所述步骤S2.9(变步长机制扩展)具体为:
S2.9.1将OPEN_again集合中的结点依次取出,作为当前结点s_again;
S2.9.2对于当前结点s_again,沿着水平、垂直、及对角方向,在特定步长范围内以步长递增的形式逐个栅格结点进行碰撞检测,直到遇到障碍物或达到特定步长Step_max,将相距最远的无碰撞结点作为当前结点s_again的邻居结点;
S2.9.3判断当前结点s_again的邻居结点s_n_again的路径代价g(s_n_again)是否小于s_n_again结点之前的路径代价,若是,则进一步判断其f(s_n_again)是否不大于f_bound,若是,则更新结点s_n_again的g值,将s_n_again的父结点设置为s_again结点,并将s_n_again结点放入OPEN列表。
经过上述步骤后,图8a-e中显示了具体实施例中基于变步长机制的任意时路径规划方法和传统算法得到的路径结果图以及各个算法的运行时间柱状图。在本次具体实施例中,规划空间网格数为150*100,分别使用A*、ARA*、AD*和ARaA*(本发明所提出的方法)进行了对比实验验证;为了分别进行了四次路径搜索,A*、ARA*和AD*的膨胀因子均设置为2.5、2、1.5和1,ARA*的初始搜索步长设置为27,按照1/3的幂律递减,共有27、9、3、1四种步长。图8a和图8d显示A*和ARaA*均能给出四条路径,而图8b和图8c显示的结果表明ARA*和AD*在该具体实施例中只能给出两条路径。参考图8e,图8e为四种方法每次进行路径搜索所需要时间的比较柱状图;从图中可以看出本发明所提出的方法搜索时间较少,尤其在使用较大搜索步长时,本发明所提出的方法搜索耗时更少且生成路径更平滑。
上面对本发明实施例结合附图进行了说明,但本发明不限于上述实施例,还可以根据本发明的发明创造的目的做出多种变化,凡依据本发明技术方案的精神实质和原理下做的改变、修饰、替代、组合或简化,均应为等效的置换方式,只要符合本发明的发明目的,只要不背离本发明的技术原理和发明构思,都属于本发明的保护范围。
Claims (3)
1.一种基于变步长机制的任意时路径规划方法,其特征在于,利用了椭圆约束进行多次路径搜索,具体包括以下步骤:
步骤S1:根据激光雷达感知端传感器获取的环境信息建立网格地图,标记每个障碍物占据的栅格,并设定路径规划的起始点S、目标点G、用于搜索的时间、最大搜索步长Step_max,将评价函数f(s)的最大约束值f_max初始化为无穷大;
步骤S2:引入变步长机制的Search_radiation_bound算法,判断最大搜索步长是否不小于最小步长且有剩余搜索时间,若是,继续执行步骤S3;否则,输出当前路径P_current并结束路径规划;
步骤S3:根据步骤S2变步长机制的Search_radiation_bound算法进行路径搜索,获得路径P,该路径是由一系列网格结点即路径点组成;
步骤S4:根据步骤S3生成路径P的路径点集合分别计算其中除起始结点和目标结点外各个结点的评价函数值f(s),按照从小到大的顺序将其存储于约束值集合f_Basic_points,并将评价函数的最大约束值f_max设置为当前路径目标结点的路径代价g(G);
步骤S5:减小当前最大搜索步长Step_max,具体可以根据实际应用场景选择递减形式;返回步骤S2;
所述步骤S2具体为:
在进行单次搜索过程中需要维护两个存储列表即OPEN列表、CLOSED列表,和两个存储集合即OPEN_again集合、f_Basic_points集合;
S2.1将CLOSE列表和OPEN_again集合初始化为空,并将规划的起始结点放入OPEN列表;判断是否为第一次规划,若是,则将评价函数值约束值f_bound初始化为f_max,否则,将f_bound初始化为约束值集合f_Basic_points中最小的f值,并将该最小的f值从f_Basic_points中删除;
S2.2判断OPEN列表与OPEN_again是否同时为空,若是,则输出路径规划失败,终止搜索;否则,执行步骤S2.3;
S2.3选取OPEN列表中评价函数值最小的结点作为当前结点s,然后将当前结点s从所述OPEN列表中删除并放入CLOSED列表;
S2.4判断当前结点s是否为目标结点,若是,则得到Step_max搜索步长下的规划路径P,结束步骤S2;否则,执行步骤S2.5;
S2.5沿着水平、垂直、及对角方向,依次对与当前结点s相距Step_max个栅格距离的八个邻居结点进行碰撞检测,对于没有发生碰撞的邻居结点s_n,若其路径代价g(s_n)小于该邻居结点s_n之前的路径代价g_old(s_n)且评价函数f(s_n)不大于评价函数约束值f_bound,则执行步骤2.6;对当前结点s的邻居结点遍历完毕后,执行步骤S2.7;
S2.6对结点s_n进行可行性检测,若其处于可达域中,则更新结点s_n的g值,将当前结点s设置为s_n的父结点,并将s_n结点放入OPEN列表;否则,继续遍历下一个邻居结点;
S2.7判断当前结点s与其邻居结点的碰撞次数是否不少于7次,或者是否存在邻居结点s_n的路径代价g(s_n)小于s_n之前的路径代价g_old(s_n)但是评价函数f(s_n)大于评价函数约束值f_bound,若是,则将当前结点s放入OPEN_again集合;
S2.8判断OPEN列表是否有新增结点,若否,且存在邻居结点s_n的路径代价g(s_n)小于s_n之前的路径代价g_old(s_n)但是评价函数f(s_n)大于评价函数约束值f_bound,则将f_bound更新为约束值集合f_Basic_points中最小的f值,并将该最小f值从f_Basic_points中删除;若约束值集合为空,则将f_bound更新为f_max;
S2.9判断OPEN列表是否为空,若是,则将当前结点s放入OPEN_again集合并对该集合中的所有结点进行变步长扩展,然后将OPEN_again集合置为空,返回步骤S2.2。
2.根据权利要求1所述的一种基于变步长机制的任意时路径规划方法,其特征在于,所述步骤S2.6可行性检测,具体为:
S2.6.1根据当前结点s的位置信息获取固定翼无人机的飞行状态;
S2.6.2结合网格地图的分辨率,将结点s_n的位置信息作为关键点进行追踪,通过解算固定翼无人机非线性动力学与控制模型获取固定翼无人机的飞行轨迹;
S2.6.3检查固定翼无人机的飞行轨迹是否经过有障碍物的栅格,若是,则结点s_n处于不可达域内,否则,结点s_n处于可达域内。
3.根据权利要求1所述的一种基于变步长机制的任意时路径规划方法,其特征在于,所述步骤S2.9的变步长扩展具体为:
S2.9.1将OPEN_again集合中的结点依次取出,作为当前结点s_again;
S2.9.2对于当前结点s_again,沿着水平、垂直、及对角方向,在特定步长范围内以步长递增的形式逐个栅格结点进行碰撞检测,直到遇到障碍物或达到特定步长Step_max,将相距最远的无碰撞结点作为当前结点s_again的邻居结点;
S2.9.3判断当前结点s_again的邻居结点s_n_again的路径代价g(s_n_again)是否小于s_n_again结点之前的路径代价,若是,则进一步判断其评价函数f(s_n_again)是否不大于评价函数约束值f_bound,若是,则更新结点s_n_again的g值,将s_again结点设置为s_n_again的父结点,并将s_n_again结点放入OPEN列表。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310624432.9A CN116817947B (zh) | 2023-05-30 | 2023-05-30 | 一种基于变步长机制的任意时路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310624432.9A CN116817947B (zh) | 2023-05-30 | 2023-05-30 | 一种基于变步长机制的任意时路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116817947A CN116817947A (zh) | 2023-09-29 |
CN116817947B true CN116817947B (zh) | 2024-02-23 |
Family
ID=88119539
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310624432.9A Active CN116817947B (zh) | 2023-05-30 | 2023-05-30 | 一种基于变步长机制的任意时路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116817947B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117451057B (zh) * | 2023-12-25 | 2024-03-12 | 长春理工大学 | 基于改进a*算法无人机三维路径规划方法、设备和介质 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106970648A (zh) * | 2017-04-19 | 2017-07-21 | 北京航空航天大学 | 城市低空环境下无人机多目标路径规划联合搜索方法 |
CN114296474A (zh) * | 2021-12-10 | 2022-04-08 | 国网新疆电力有限公司超高压分公司 | 一种基于路径时间代价的无人机路径规划方法及系统 |
CN114877905A (zh) * | 2022-05-10 | 2022-08-09 | 沈阳大学 | 一种双向动态生长的Informed-RRT*路径规划方法 |
WO2022193584A1 (zh) * | 2021-03-15 | 2022-09-22 | 西安交通大学 | 一种面向多场景的自动驾驶规划方法及系统 |
CN115268456A (zh) * | 2022-08-10 | 2022-11-01 | 哈尔滨理工大学 | 一种动态变策略informed-RRT*的无人车路径规划方法 |
CN115741686A (zh) * | 2022-11-14 | 2023-03-07 | 江苏科技大学 | 一种基于变概率约束采样的机器人路径规划方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6606442B2 (ja) * | 2016-02-24 | 2019-11-13 | 本田技研工業株式会社 | 移動体の経路計画生成装置 |
US11727812B2 (en) * | 2017-07-27 | 2023-08-15 | Beihang University | Airplane flight path planning method and device based on the pigeon-inspired optimization |
-
2023
- 2023-05-30 CN CN202310624432.9A patent/CN116817947B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106970648A (zh) * | 2017-04-19 | 2017-07-21 | 北京航空航天大学 | 城市低空环境下无人机多目标路径规划联合搜索方法 |
WO2022193584A1 (zh) * | 2021-03-15 | 2022-09-22 | 西安交通大学 | 一种面向多场景的自动驾驶规划方法及系统 |
CN114296474A (zh) * | 2021-12-10 | 2022-04-08 | 国网新疆电力有限公司超高压分公司 | 一种基于路径时间代价的无人机路径规划方法及系统 |
CN114877905A (zh) * | 2022-05-10 | 2022-08-09 | 沈阳大学 | 一种双向动态生长的Informed-RRT*路径规划方法 |
CN115268456A (zh) * | 2022-08-10 | 2022-11-01 | 哈尔滨理工大学 | 一种动态变策略informed-RRT*的无人车路径规划方法 |
CN115741686A (zh) * | 2022-11-14 | 2023-03-07 | 江苏科技大学 | 一种基于变概率约束采样的机器人路径规划方法 |
Non-Patent Citations (2)
Title |
---|
UAV Formation Transformation Based on Two-Stage Path Planning Method;Bingyu Song et al.;2022 4th International Conference on Artificial Intelligence and Computer Science;20221231;全文 * |
宋晓琳 ; 周南 ; 黄正瑜 ; 曹昊天 ; .改进RRT在汽车避障局部路径规划中的应用.湖南大学学报(自然科学版).2017,(第04期),全文. * |
Also Published As
Publication number | Publication date |
---|---|
CN116817947A (zh) | 2023-09-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111780777B (zh) | 一种基于改进a*算法和深度强化学习的无人车路径规划方法 | |
CN108458717A (zh) | 一种迭代的快速扩展随机树irrt的无人机路径规划方法 | |
CN112987799B (zh) | 一种基于改进rrt算法的无人机路径规划方法 | |
CN109597425B (zh) | 基于强化学习的无人机导航和避障方法 | |
CN108827335B (zh) | 一种基于单向搜索模型的最短路径规划方法 | |
CN116817947B (zh) | 一种基于变步长机制的任意时路径规划方法 | |
CN108444490B (zh) | 基于可视图和a*算法深度融合的机器人路径规划方法 | |
CN110456825B (zh) | 一种基于改进快速随机搜索树的无人机在线运动规划方法 | |
CN113359718B (zh) | 移动机器人全局路径规划与局部路径规划融合方法及设备 | |
CN109931942A (zh) | 机器人路径生成方法、装置、机器人和存储介质 | |
CN111723983B (zh) | 一种未知环境下无人机的时间参数化航路规划方法与系统 | |
CN109211242B (zh) | 一种融合rrt与蚁群算法的三维空间多目标路径规划方法 | |
CN113359775B (zh) | 一种动态变采样区域rrt无人车路径规划方法 | |
CN113110601B (zh) | 一种无人机电力线路巡检路径优化方法及装置 | |
CN112344938B (zh) | 基于指向和势场参数的空间环境路径生成及规划方法 | |
CN114791743A (zh) | 一种考虑通信时延的无人机集群协同航迹规划方法 | |
CN115407784B (zh) | 一种基于空地信息互补的无人车路径规划方法 | |
CN113188555A (zh) | 一种移动机器人路径规划方法 | |
CN116203965A (zh) | 一种基于agv的改进蚁群算法的路径规划算法 | |
CN114237302B (zh) | 一种基于滚动时域的三维实时rrt*航路规划方法 | |
CN116182859A (zh) | 路径规划方法及装置、设备、存储介质 | |
CN114428499A (zh) | 一种融合Astar与DWA算法的移动小车路径规划方法 | |
CN116627160A (zh) | 一种未知环境下多旋翼无人机在线航迹规划方法 | |
CN114879665A (zh) | 用于车辆远程召唤系统的安全节能运动规划方法及系统 | |
Zhu et al. | Path planning of mobile robot based on deep reinforcement learning with transfer learning strategy |
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 |