0 引言
路径规划按层次结构可分为全局路径规划[6]和局部路径规划[7]。全局路径规划可离线生成一条连接起点和终点的安全路径,实现对静态障碍物的避障。目前,常用的全局路径规划方法包括基于图搜索的算法[8][9]、基于采样的探索树算法[10]、群智能算法[11][12],以及基于这些算法的改进算法[13][14]。文献[15]通过改进邻节点的搜索方法提高了算法的运行速度,但在生成大转折角度路径时,该算法的速度往往会降低。文献[16]通过将3×3领域扩展为5×5领域,并在16个搜索方向中舍弃了7个方向节点,以提高路径的平滑程度,但代价是增加了计算量。文献[17]通过优化启发函数剔除了路径中的冗余节点,提高了局部规划算法的执行速度,但该方法需要占用较多的内存。以往的研究多注重提高算法的速度和路径的平滑程度,对全局路径的可执行性大多依赖于障碍物地图膨胀。然而,在狭小空间下,障碍物地图膨胀往往会导致规划失败,或者使所规划出的全局路径在后续执行过程中在拐角处与障碍物发生碰撞。
在存在随机障碍物和动态障碍物的复杂场景中,全局规划算法无法有效避障。因此,常配合采用局部路径规划算法来进行局部避障。动态窗口法[18][19]是一种被广泛使用的局部规划方法,动态窗口法通过在控制空间进行采样来确定速度范围。首先,根据无人机与障碍物的相对距离、硬件限制以及加减速度限制,确定速度采样空间。然后,将速度采样空间应用于无人机的运动学模型,生成多条预测轨迹。最后,通过评价函数对这些轨迹进行打分,选出最优轨迹供无人机执行。文献[20][21]结合全局路径数据和动态窗口法,使无人机达到目标点。文献[22]通过改进评价函数,使无人机能提前规避C型障碍物。文献[23]通过引入动态障碍物信息,优化了动态窗口法的评价函数,从而提升了避障能力。但该方法在应对高速动态障碍物时仍有改进空间。现有文献大多研究低速动态障碍物的避障方法,但这些方法对高速动态障碍物的避障能力有限。为此,本文提出了一种融合改进算法和改进动态窗口法的全局路径规划算法,实现了无人机的路径规划。该方法不仅考虑了全局路径规划的路线优劣,还在局部动态规划时改进了算法,以应对动态障碍物规避不及时的问题,从而提升无人机对高速动态障碍物的避障能力。
1 改进A*算法
A*算法是一种常用的路径规划算法,在较为复杂环境下A*算法由于其具有遍历属性其在如街道及建筑等结构化场景下效率高于采样类算法,同时在该复杂场景下其运算时间远高于如遗传算法、粒子群优化等优化类算法。它通过维护一个开放列表和一个封闭列表搜索周围邻居的最优节点来寻找最优路径。该算法从起点开始,计算每个邻居节点到起点的代价与到终点的估计代价之和作为总代价,并选择总代价最小的节点作为下一个扩展节点。这个过程不断重复,直到终点被访问,整个搜索过程结束。
1.1 搜索路径优化
传统A*算法搜索路径紧贴障碍物,使得后续的局部规划难以进行。工程上常采用设置障碍物膨胀层的方法来解决此问题。过程较为复杂,并且在狭窄空间内往往导致规划失败。针对这一问题,本文提出了一种基于改进A*算法代价函数的方法。传统A*的代价值由两部分组成:F(n)=G(n)+H(n),其中G(n)和H(n)分别表示当前节点到起点的代价值和当前节点到终点启发函数值。启发函数一般采用欧式距离或曼哈顿距离。本文提出了一种新的代价函数来解决此问题,代价函数表示为F(n)=G(n)+H(n)+D(n)其中G(n)、H(n)与传统A*相同,H(n)采取当前节点到终点的曼哈顿距离,D(n)表示节点与最近障碍物的相对距离项。
H(n)=$\left|{x}_{n}-{x}_{g}\right|$+$\left|{y}_{n}-{y}_{g}\right|$
D(n)=M-min{dist(node,ob)}
其中M为表示扩展路径与障碍物的相对距离系数,min{dist(node,ob)}表示当前点与周围障碍物节点的最小距离,通过增加D(n)项使得算法搜索最优节点时评估节点与障碍物的距离,节点与障碍物之间最小距离接近于M时D(n)取得最小,使得搜索出的路径与静态障碍物之间保持一定距离。
dist(node,ob)=$\sqrt{({x}_{n}-{x}_{ob}{)}^{2}-({y}_{n}-{y}_{ob}{)}^{2}}$
建立60×60的栅格地图,传统A*、RRT算法改进A*规划结果对比如下。
仿真结果可发现RRT算法由于其算法随机性性质限制其无效扩展路径较多,所花费的运行为18秒远高于两种A*算法。
传统规划算法以追求路径长度最小化为目标,导致生成的路径紧贴在障碍物表面。这会引起后续无人机的跟踪困难,并且在遇到动态障碍物时会因避障空间有限,导致避障困难。相比之下,改进后的路径与障碍物之间保持一定距离,有利于无人机的跟踪以及动态避障。
1.2 仅保存关键点的冗余点删除策略
1.2.1 同一直线冗余点删除
上述方法的改进提升了路径的可执行性,但由于算法搜索出的路径是在离散空间内进行,包含大量冗余节点,这些节点在进行局部规划时会形成大量子目标点,进而增加了运行时间并对无人机的运动稳定性造成影响。为解决此问题,本文提出一种冗余点删除策略。新策略利用向量叉乘法遍历路径上的所有节点,识别并删除位于同一直线中间的冗余点。只保留处于同一直线路径上两端点。通过这种处理,不仅保留了关键的转折点,同时减少了路径长度,还为后续无人机的跟随节省了大量时间。基于新的策略,无人机在导航时能够更加流畅地进行运动,避免不必要的停顿和加减速,从而提高整体的效率和性能。
1.2.2 冗余拐点删除
在进行局部规划时,经改进A*规划出的路径虽然可保证无人机能够安全运行,但会产生过多拐点导致无人机反复加减速,从而增加了运行时间。为此,本文提出一种冗余拐点删除策略。具体方法为:从起点开始遍历所有拐点,若相邻两拐点距离小于某一距离阈值且连线不经过障碍物,则删除第二个拐点,该阈值与无人机动力学模型相关,然后继续遍历下一个拐点。若仍满足距离小于该距离阈值且连线不经过障碍物的条件,则继续删除,直至距离大于等于该距离阈值,则保留该拐点,直到遍历完整条路径。
2 改进动态窗口法
动态窗口法是无人机路径规划中常用的一种方法,主要用于局部路径规划。传统的动态窗口法在处理随机障碍物和动态障碍物时具有一定的避障能力。然而,当动态障碍物的运动速度过快时,传统的动态窗口法往往无法及时规避,可能导致无人机与障碍物发生碰撞。针对于此,本文提出了一种基于评价函数的改进方法。
2.1 运动模型
动态窗口法通过接受上一时刻传入的线速度和角速度,确定无人机在下一时刻的速度范围。将这个速度范围传入运动学模型进行轨迹预测。假设在时刻 t,无人机的线速度为 v(t),角速度为ω(t),位姿在坐标系中表示为x(t),y(t),θ(t),则下一时刻的位姿可以表示为:
$\left\{\begin{array}{l}x(t+1)={x}_{t}+{v}_{t}\mathrm{c}\mathrm{o}\mathrm{s}\left({\theta }_{t}\right)\\ y(t+1)={y}_{t}+{v}_{t}\mathrm{s}\mathrm{i}\mathrm{n}\left({\theta }_{t}\right)\\ \theta (t+1)={\theta }_{t}+{w}_{t}\mathrm{\Delta }t\end{array}\right.$
2.2 运动速度采样
运动速度范围由无人机自身硬件最大最小速度限制、电机加减速限制、刹车距离限制决定。
自身硬件限制,其中Vmin、Vmax、ωmin、ωmax分别为无人机本身最小最大线速度以及最小最大角速度。
Vm=$\left\{\begin{array}{l}v\in [{V}_{\mathrm{m}\mathrm{i}\mathrm{n}},{V}_{\mathrm{m}\mathrm{a}\mathrm{x}}]\\ \omega \in [{\omega }_{\mathrm{m}\mathrm{i}\mathrm{n}},{\omega }_{\mathrm{m}\mathrm{a}\mathrm{x}}]\end{array}\right\}$
电机加减速能力限制,其中${\stackrel{·}{v}}_{b}$为最大线减速度,${\stackrel{·}{v}}_{a}$为最大线加速度,${\stackrel{·}{w}}_{b}$为最大角减速度,${\stackrel{·}{w}}_{a}$为最大角加速度。
$\begin{array}{l} V_{a}=\left\{(v, \omega) \mid v \in\left[v_{c}-\dot{v}_{b} \Delta t, v_{c}+\dot{v}_{a} \Delta t\right],\right. \\ \omega \in\left[\omega_{c}-\dot{w}_{b} \Delta t, \omega_{c}+\dot{w}_{a} \Delta t\right\} \end{array}$
刹停距离限制,当出现障碍物时需保证在与其碰撞前停下,其中dist(v,ω)为无人机当前位置到障碍物的距离。
Vd={(v,ω)|v≤$\left(2dist\right(v,w){\stackrel{·}{v}}_{b}{)}^{\frac{1}{2}}$,$\omega \le \left(2\mathrm{d}\mathrm{i}\mathrm{s}\mathrm{t}\right(v,\omega ){\stackrel{·}{\omega }}_{b}{)}^{\frac{1}{2}}$}
由以上三项确定无人机的速度范围为Vr=Vm∩Va∩Vd,在此速度范围内可保证无人机在满足自身硬件限制、并且避开障碍物的前提下选择合适的角速度和线速度以供后续的轨迹预测。
2.3 评价函数及其改进
动态窗口法通过传入速度范围和运动学模型,模拟生成多条路径,并通过评价函数对这些路径进行评分,以选出最优路径。传统动态窗口法的评价函数通常由三个部分组成:
G(v,ω)=αP(v,ω)+βD(v,ω)+γV(v,ω)
其中P(v,ω)表示π减去窗口轨迹中每条轨迹末端无人机轨迹末端朝向终点的角度误差之值,D(v,ω)表示轨迹与最近障碍物的最小距离,若无障碍物则设定为一较大值,V(v,ω)表示轨迹末端的速度,α、β、γ为各项评价函数的权重,选出打分最高的轨迹进行输出控制量。动态窗口法对静态或者低速障碍物避障的规避效果较理想,但是当动态障碍物速度较大时,动态窗口法所计算的速度范围往往不能满足避障需求。针对这个问题,本文提出了一种新的评价函数:
G(v,ω)=αP(v,ω)+βD(v,ω)+γV(v,ω)+δDy(v,ω)
其中P(v,ω)、D(v,ω)、V(v,ω)与传统动态窗口法相同,附加项dy(v,ω)=ob_tra_dist+tra_dis,如图7所示,算法调用流程为当无人机当前位置与动态障碍物的距离distance2小于设定阈值时,判断动态障碍物是否位于以无人机当前朝向角度为theta的包角范围内。如果满足该条件,则进一步判断distance1是否小于另一个设定的避障阈值。当且仅当这三个条件同时满足时,计算Dy(v,ω)=ob_tra_dist+tra_dist并将该评价值传入总评价函数中进行新轨迹的选择,算法伪代码如图6所示。算法执行时首先将窗口采样轨迹传入评价函数模块并带入轨迹评价函数进行打分。如图7,附加项中ob_tra_dist表示预测轨迹末端与动态障碍物之间距离的最大值,tra_dist表示预测轨迹末端距离以无人机自身方向角和自身位置所确定直线的最大距离。如图7所示,其原理可解释为当无人机运动时,在没有动态障碍物的情况下通常沿直线行驶。通过监测无人机的实时位置和姿态,可以计算出在无障碍物情况下的下一个时间段的预测轨迹,从而提前预测无人机轨迹是否存在碰撞风险,并进行相应的避障措施。这样可以防止无人机在接近障碍物时因响应不及而发生碰撞。
其轨迹具体选择示意图如图6所示:
3 仿真验证与分析
为验证算法效果,本文建立的栅格地图,其中黑色代表静态障碍物,红色代表移动障碍物。无人机宽为0.6m,长为0.6m,本文设置四组仿真实验用于对比,设起点坐标为S(5,2),终点坐标为G(55,4),并添加动态障碍物,其中动态障碍物一速度为2m/s,方向向下;动态障碍物二速度为4m/s,方向向上;动态障碍物三和动态障碍物四速度大小为1.8m/s,方向向左;动态障碍物五速度大小为2m/s,方向向。DWA 算法中无人机最大线速度为5m/s,最大角速度为50°/s,最大线加速度为5m/s2,最大角加速度为100°/s2,评价函数的各个参数为α=4.0,β=2.0,γ=1.0,δ=4.0。为模拟真实环境,本文所建立的地图模拟了走廊环境以及随机障碍物环境,其布局如图所示。
图9为基于传统规划方法生成的路径。仿真结果显示,在执行过程中,无人机可能会紧贴障碍物并且在拐角处产生过大的转折角度,导致无人机无法安全通过,并与障碍物发生碰撞。这种情况会影响后续算法的执行,进而导致路径规划失败,故不可直接采用传统A*进行全局路径规划。
算法一:改进A*融合传统DWA未剔出冗余拐点。
算法二:改进A*融合改进DWA未剔出冗余拐点。
算法三:改进A*融合传统DWA剔出冗余拐点。
算法四:改进A*融合改进DWA剔出冗余拐点。
dist1:距离第一个动态障碍物的最小距离。
dist2:距离第二个动态障碍物的最小距离。
dist3:距离第三个动态障碍物的最小距离。
dist4:距离第四个动态障碍物的最小距离。
dist5:距离第五个动态障碍物的最小距离。
表1 算法结果参数对比Table 1 Comparison of algorithm parameters |
| algorithm | Time/s | path length/m | dist1 /m | dist2 /m | dist3 /m | dist4 /m | dist5 /m |
|---|---|---|---|---|---|---|---|
| One | 125.1 | 247.3 | 0.13 | 0.86 | 1.35 | 0.86 | 1.21 |
| Two | 127.1 | 247.1 | 1.56 | 1.84 | 4.66 | 4.04 | 1.81 |
| Three | 98.1 | 241.4 | 0.08 | 0.71 | 3.40 | 2.75 | 0.59 |
| Four | 99.5 | 242.4 | 1.48 | 1.68 | 3.92 | 3.29 | 1.67 |
对运行时间进行分析可知,删除冗余拐点后的算法较未删除冗余拐点的算法平均减少了21.8%的时间消耗。轨迹长度方面,删除冗余拐点后的算法仅增加了2.1%轨迹长度,对无人机跟踪性能的影响较小。
通过速度图的分析发现,剔除冗余拐点后的算法相较未剔除时,无人机的速度变化率明显降低。无人机的加速度更加平稳,有助于节省能量。在无人机的安全距离方面,相较于传统算法,改进后的算法展现出出色的避障能力,特别是对于动态障碍物,无人机能够进行提前规避,改进动态窗口法相比较于传统动态窗口法,在面对动态障碍物时安全距离大大提高,确保无人机安全运行。
4 结束语
为解决无人机避障问题,本文提出了一种创新性的融合算法,将改进A*算法与改进动态窗口法相结合,实现对动、静态障碍物的避障。针对传统A*算法所规划的路径过于靠近静态障碍物的问题提出了一种新的处理方法,通过改进评价函数,改进A*算法,使其能够结合先验地图直接输出安全路径。同时为解决输出的全局路径点过多的问题提出了冗余路径点、冗余拐点删除策略。实验表明,该策略在后续局部规划执行过程中轨迹仅增加2.1%的基础上运行时间减少21.8%。
传统动态窗口法仅适用于低速场景,但在面对较高速动态障碍时易陷入局部最优导致避障能力急剧下降。针对此问题本文提出了改进动态窗口法,通过预先规避实现对动态障碍物的避障,提高了无人机运行过程中的安全性。
算法仿真验证表明,本文方法能够有效实现对动、静态障碍物的避障,有效地解决传统A*的路径安全性差问题、运行时间过长问题、以及动态窗口法在面对较高速动态障碍物时避障能力下降的问题,这三个问题的解决对与无人机安全性的提高、能量节省、以及行驶平稳以及具有重要作用。