一种基于自适应参数的改进路径规划方法

1.本发明涉及智能体路径规划技术领域，具体指一种基于自适应参数的改进路径规划方法。


背景技术：

2.移动智能体近年来取得了丰硕的成果，应用场景广泛，涵盖自主水下航行器(auv)、无人地面车辆(ugv)和无人机(uav)等。由多个移动智能体合作探索或移动时，需要考虑的一个基本且重要的问题是路径规划问题，其旨在根据不同的环境、智能体的运动限制和其他因素设计和部署合适的算法。
3.目前，常见的技术手段能够对静态的未知环境完成最优的路径规划，并且精确度高，能够完美的避开障碍物。但是，在现实中未知环境往往是动静态相结合的环境。因此，常规的路径规划方法无法高精度避开动态障碍物。
4.为解决上述技术问题，有人选取动态窗口算法(dwa)，通过该算法在未知环境中为智能体规划路径，并且具备实时避障能力，从而实现在未知环境中完成动态障碍物和静态障碍物的避障功能。但是为了实现实时计算当前环境，从而导致计算量非常大。因此，近年来，对于dwa算法不断的改进，主要包括计算量的改进、窗口精确度改进、不规则障碍物的边界定义改进和评价函数参数自适应改进等，并且都有了很好的进展。
5.但是，现有的选取dwa的路径规划方法仍然存在一下缺陷：
6.1、面对狭小缝隙障碍物时，原算法由于无效轨迹的过快迭代判断和有限的预测轨迹不满足穿越缝隙的需求，会导致突然减速和抖动现象。
7.2、在实际环境中，经常会出现各种各样的陷阱障碍物，如非凸区域陷阱、直线陷阱等。其中，如图2右所示的直线陷阱障碍物是指智能体、障碍物和目标点在一条直线上。在这种情况下，由于对称性，智能体不能主动转向绕过障碍物。此外，图2左所示的非凸区域陷阱的情况可能出现在仓库、房屋和其他结构中。在利用有限的视觉传感器或其他环境传感器获取局部信息时，很难在落入非凸区域陷阱之前发出警告，也很难在陷阱外减速。
8.3、由于动态障碍物在未知环境下难以被数学定义，只能通过智能体传感器获取其相对位置，致使原dwa算法往往无法避免侧向碰撞。
9.4、其中对现有的评价函数参数自适应改进只针对一种环境下的速度权重，没有考虑到稠密障碍物环境与狭小缝隙障碍物环境等其他环境种类，以及评价函数中的航向角评价函数与距离评价函数，从而导致路径得规划仍然存在一定得缺陷。


技术实现要素：

10.本发明根据现有技术的不足，提出一种基于自适应参数的改进路径规划方法，实现在包含大量静态和动态障碍物、路径约束和性能约束的复杂环境中为智能体规划实时路径。
11.为了解决上述技术问题，本发明的技术方案为：
12.一种基于自适应参数的改进路径规划方法，包括如下步骤：
13.s1、通过dwa算法进行初始路径的规划；
14.s2、在初始路径的基础上，针对不同障碍物种类设计评价函数参数的权重并制得转向策略，
15.s2-1、当智能体在稠密障碍物间穿行时，通过设计自适应率减小航向项对初始路径的轨迹的影响，其中航向角自适应率α(t)的表达式如下：
[0016][0017]
其中，λ1＞1是航向角的自适应参数的权重，p(t)为t时刻智能体的位置，ob(t)指t时刻距离智能体最近的障碍物位置，ζ1指距离阈值，ρ1为调整自适应变化速率的可调参数之一，设定ρ1＞1；
[0018]
s2-2、面对狭小缝隙障碍物时，降低评价函数中距离参数的权重，其中距离自适应率β(t)的设计如下：
[0019][0020]
其中，λ2＞0是距离自适应参数的权重；
[0021]
s2-3、面对陷阱障碍物时，在dwa算法中制定转向策略，其中转向策略定义如下：
[0022][0023]
其中，ω
turn
是智能体在单位时间t内的转动角度，智能体逆时针或顺时针转动，分别由ω
turn
＞0和ω
turn
＜0决定；
[0024]
s3、针对动态环境下的移动障碍物进行预测避障。
[0025]
作为优选，所述步骤s1包括如下子步骤：
[0026]
s1-1在预设时间间隔内通过对速度v和角度ω的积分求解获得智能体在下一时刻的状态；
[0027]
s1-2、计算智能体在预设时间间隔内能移动的可行速度范围vd；
[0028]
s1-3、考虑与障碍物之间的安全刹车距离限制的可行速度范围va；
[0029]
s1-4、获取智能体本身存在的物理性能限制的最大速度空间vs；
[0030]
s1-5、结合可行速度范围vd、可行速度范围va以及最大速度空间vs，得到三者交集，即智能体可选的速度窗口vr，表示为：vr＝vd∪va∪vs；
[0031]
s1-6、通过评价函数遍历智能体可选的速度窗口vr中每一条速度。
[0032]
作为优选，所述步骤s1-1中下一时刻的状态包括绝对坐标、速度、移动角度。
[0033]
作为优选，所述步骤s1-1中，智能体在下一时刻的状态的获取方法：
[0034]
定义智能体的一般运动方程：
[0035][0036]
其中t0示初始时间，v表示智能体速度，θ表示机器人航向角度，以水平线为起始角度，x(tn)表示tn时刻智能体的x坐标，
[0037]
同时，还可得到一个完全离散形式的表达式：
[0038]
[0039]
其中，
[0040][0041]
当角速度ωi＝0时，智能体将沿直线运动，ωi≠0时做以为圆心，为半径的圆周运动：
[0042][0043][0044][0045]
作为优选，所述步骤s1-2中，智能体在预定时间间隔内能移动可行速度范围vd的获取方法：
[0046][0047]
其中，(va,ωa)表示智能体实际速度与角速度。
[0048]
作为优选，所述步骤1-3中，考虑与障碍物之间的安全刹车距离限制的可行速度范围va的获取方法：
[0049][0050]
其中，其中dist(v,ω)表示在速度v和角速度ω的情况下，智能体距离最近障碍物的距离。
[0051]
作为优选，所述步骤s1-6中，遍历智能体可选的速度窗口vr中的每一条速度的方法：
[0052]
g(v,ω)＝σ(α
·
heading(v,ω) β
·
dist(v,ω) γ
·
velocity(v,ω))
[0053]
其中，α，β，γ分别是权重系数，gvelocity(v,ω)代表智能体的速度得分，heading(v,ω)表示智能体的航向角得分，dist(v,ω)表示智能体与最近障碍物之间的距离得分，三项评价指标将通过σ(
·
)归一化处理。
[0054]
作为优选，所述步骤s3包括如下子步骤：
[0055]
s3-1、观测阶段：在有限的传感器检测范围中捕获移动障碍物在tn时刻的绝对坐标持续观测m-n(m＞n)时间片建立位置信息，即坐标矩阵
[0056][0057]
s3-2、预测阶段：对储存好的位置信息进行预测，采用基于最小二乘思想的高阶拟合函数对其横纵坐标分别预测，得到拟合系数组合
[0058]
由于动态障碍物的不规则运动，固定的拟合系数不足以准确预测动态障碍物的运
动趋势，因此，tm时间后，采用滚动窗口的思想，实时更新和准确地计算出动态障碍物的运动趋势，直到障碍物超出传感器的观测范围；
[0059]
s3-3、应用阶段：通过预测得到动态障碍物在tm s*δt时间后的位置并将其作为虚拟静态障碍物添加到智能体的障碍物地图中，并实时更新。
[0060]
作为优选，所述步骤s3-2中，预测过程是基于时间序列的，即利用当前时间段记录的位置信息来预测障碍物未来可能的位置其中ls为基于最小二乘法的拟合函数。
[0061]
本发明具有以下的特点和有益效果：
[0062]
通过对dwa算法中的评价函数权重自适应优化，使得智能体在面对稠密障碍物和狭小缝隙障碍物时，能够快速且安全的通过。设计的转向策略还可以帮助智能体大概率逃出非凸区域陷阱障碍物和直线障碍物。
[0063]
此外，基于对动态障碍物的路径跟踪，通过拟合预测的方法得到动态障碍物的未来运动趋势，减少智能体在动态环境中发生碰撞的概率，避免大多数侧向碰撞的情况。
附图说明
[0064]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0065]
图1为本实施例中穿过狭小缝隙障碍物的具体步骤示意图。
[0066]
图2为本实施例中直线陷阱障碍物和非凸区域陷阱障碍物两种陷阱障碍物类型示意图。
[0067]
图3为本实施例中通过转向策略逃出的陷阱障碍物示意图。
[0068]
图4为本实施例中不同障碍物密度下改进dwa算法优于原算法的次数示意图。
[0069]
图5为本实施例中不同障碍物密度下两种算法的失败次数和平均转向次数对比示意图。
[0070]
图6为本实施例中动态障碍物轨迹预测步骤流程图。
[0071]
图7为本实施例中带预测避障的动态障碍物避障路线。
具体实施方式
[0072]
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
[0073]
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相
对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”等的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。
[0074]
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以通过具体情况理解上述术语在本发明中的具体含义。
[0075]
本发明提供了一种基于自适应参数的改进路径规划方法，如图6所示，包括如下步骤：
[0076]
s1、通过dwa算法进行初始路径的规划。
[0077]
具体的，s1-1在预设时间间隔内通过对速度v和角度ω的积分求解获得智能体在下一时刻的状态，其中，下一时刻的状态包括绝对坐标、速度、移动角度，
[0078]
本实施例中易横坐标为例，首先定义智能体的一般运动方程：
[0079][0080]
其中t0示初始时间，v表示智能体速度，θ表示机器人航向角度，以水平线为起始角度，x(tn)表示tn时刻智能体的x坐标，
[0081]
同时，还可得到一个完全离散形式的表达式：
[0082][0083]
其中，
[0084][0085]
当角速度ωi＝0时，智能体将沿直线运动，ωi≠0时做以为圆心，为半径的圆周运动：
[0086][0087][0088][0089]
s1-2、计算智能体在预设时间间隔内能移动的可行速度范围vd。
[0090]
具体的，计算线加速度和角加速度有限的情况下，智能体在预定时间间隔内能移动可行速度范围vd的获取方法：
[0091]
[0092]
其中，(va,ωa)表示智能体实际速度与角速度。
[0093]
本实施例中预定时间间隔设定为1-5s。
[0094]
s1-3、考虑与障碍物之间的安全刹车距离限制的可行速度范围va；
[0095]
具体的，考虑与障碍物之间的安全刹车距离限制的可行速度范围va的获取方法：
[0096][0097]
其中，其中dist(v,ω)表示在速度v和角速度ω的情况下，智能体距离最近障碍物的距离。
[0098]
s1-4、获取智能体本身存在的物理性能限制的最大速度空间vs；
[0099]
s1-5、结合可行速度范围vd、可行速度范围va以及最大速度空间vs，得到三者交集，即智能体可选的速度窗口vr，表示为：vr＝vd∪va∪vs；
[0100]
s1-6、通过评价函数遍历智能体可选的速度窗口vr中每一条速度。
[0101]
具体的，遍历智能体可选的速度窗口vr中的每一条速度的方法：
[0102]
g(v,ω)＝σ(α
·
heading(v,ω) β
·
dist(v,ω) γ
·
velocity(v,ω))
[0103]
其中，α，β，γ分别是权重系数，gvelocity(v,ω)代表智能体的速度得分，heading(v,ω)表示智能体的航向角得分，dist(v,ω)表示智能体与最近障碍物之间的距离得分，三项评价指标将通过σ(
·
)归一化处理。
[0104]
s2、在初始路径的基础上，针对不同障碍物种类设计评价函数参数的权重并制得转向策略，
[0105]
s2-1、当智能体在稠密障碍物间穿行时，为使得智能体能尽快通过稠密障碍物，需要降低航向评分，通过设计自适应率减小航向项对初始路径的轨迹的影响，从而在稠密障碍物中移动的智能体能够在保持安全距离的同时维持稳定速度，其中航向角自适应率α(t)的表达式如下：
[0106][0107]
其中，λ1＞1是航向角的自适应参数的权重，p(t)为t时刻智能体的位置，ob(t)指t时刻距离智能体最近的障碍物位置，ζ1指距离阈值，ρ1为调整自适应变化速率的可调参数之一，设定ρ1＞1；
[0108]
s2-2、面对狭小缝隙障碍物时，降低评价函数中距离参数的权重，其中距离自适应率β(t)的设计如下：
[0109][0110]
其中，λ2＞0是距离自适应参数的权重；
[0111]
可以理解的，原算法由于无效轨迹的过快迭代判断和有限的预测轨迹不满足穿越缝隙的需求，会导致突然减速和抖动现象。适当降低评价函数中的距离部分权重，使得智能体不过于排除障碍物，从而平缓通过狭小缝隙障碍物。
[0112]
如图1所示，黑色物体为狭小缝隙障碍物，圆圈为智能体，延伸出去的箭头为通过dwa算法生成的有限预测轨迹，三个阶段分别体现了智能体从面对狭小缝隙障碍物到减速到通过的过程。
[0113]
与原算法中的固定权重不同，在图1的(a)(b)阶段设计可以动态调整的距离项权
重，使智能体不会过度排斥靠近障碍物的预测轨迹，因此智能体不会提前减速和过度振荡。
[0114]
s2-3、面对陷阱障碍物时，在dwa算法中制定转向策略，其中转向策略定义如下：
[0115][0116]
其中，ω
turn
是智能体在单位时间t内的转动角度，智能体逆时针或顺时针转动，分别由ω
turn
＞0和ω
turn
＜0决定；
[0117]
可以理解的，在实际环境中，经常会出现各种各样的陷阱障碍物，如非凸区域陷阱、直线陷阱等。
[0118]
其中，图2右所示的直线陷阱障碍物是指智能体、障碍物和目标点在一条直线上。在这种情况下，由于对称性，智能体不能主动转向绕过障碍物。此外，如图2左所示，非凸区域陷阱的情况可能出现在仓库、房屋和其他结构中。
[0119]
在利用有限的视觉传感器或其他环境传感器获取局部信息时，很难在落入非凸区域陷阱之前发出警告，也很难在陷阱外减速。因此，有必要设计局部区域内逃脱陷阱的策略。改进dwa算法引入了临界碰撞判定：当智能体以极低的速度移动时，受到dwa算法连续运动的要求，所以不能保证vr中所有预测的轨迹都是安全的，该状态被定义为临界碰撞状态。对于这类几乎静止的智能体，改进dwa算法驱动智能体原地转向并移动，进而尝试绕出凹型障碍物。
[0120]
进一步的，本实施例为验证评价函数参数自适应设计和转向策略的可行性和优越性，本发明在表1条件下分别部署了10、15、20、25、30、35个随机圆形障碍物地图，并重复仿真100次。
[0121]
表1固定参数与可调参数表
[0122][0123]
得到以下结果：
[0124]
根据图4，图5结果表明，改进dwa算法在时间，路径长度，失败次数和平均转向次数上均优于原dwa算法，尤其在障碍物密度为30个的时候效果最佳。
[0125]
s3、针对动态环境下的移动障碍物进行预测避障。
[0126]
可以理解的，由于动态障碍物在未知环境下难以被数学定义，只能通过智能体传感器获取其相对位置。此外原dwa算法往往无法避免侧向碰撞。针对这种情况，本研究提出了一种策略，即由智能体预测在其周围一定范围内的动态障碍物的移动轨迹(直线和曲线)，从而避开动态障碍物。具体实施步骤如下：
[0127]
s3-1、观测阶段：在有限的传感器检测范围中捕获移动障碍物在tn时刻的绝对坐标持续观测m-n(m＞n)时间片建立位置信息，即坐标矩阵
[0128][0129]
s3-2、预测阶段：对储存好的位置信息进行预测，采用基于最小二乘思想的高阶拟合函数对其横纵坐标分别预测，得到拟合系数组合预测过程是基于时间序列的，即利用当前时间段记录的位置信息来预测障碍物未来可能的位置其中ls为基于最小二乘法的拟合函数；
[0130]
由于动态障碍物的不规则运动，固定的拟合系数不足以准确预测动态障碍物的运动趋势，因此，tm时间后，采用滚动窗口的思想，实时更新和准确地计算出动态障碍物的运动趋势，直到障碍物超出传感器的观测范围；
[0131]
s3-3、应用阶段：通过预测得到动态障碍物在tm s*δt时间后的位置并将其作为虚拟静态障碍物添加到智能体的障碍物地图中，并实时更新。
[0132]
通过上述三级避障策略，如图7所示，上方三幅图为没有预测避障的动态障碍物避障路线；下方：带预测避障的动态障碍物避障路线，下方三幅图带预测避障的动态障碍物避障路线，明显可以看出智能体可以提前避开动态障碍物(改进的dwa算法决定减速或转弯)，减少侧面碰撞的隐患。
[0133]
以上结合附图对本发明的实施方式作了详细说明，但本发明不限于所描述的实施方式。对于本领域的技术人员而言，在不脱离本发明原理和精神的情况下，对这些实施方式包括部件进行多种变化、修改、替换和变型，仍落入本发明的保护范围内。