动态环境中基于时空栅格地图的局部路径规划方法及装置与流程

1.本发明无人平台路径规划技术领域，尤其是涉及一种动态环境中基于时空栅格地图的局部路径规划方法及装置。


背景技术：

2.近年来，无人平台被广泛应用于各类服务机器人，与机械臂等固定的工业机器人不同，无人平台需在特定环境中实现自主导航，即能够在与障碍物不发生碰撞的条件下安全到达目标点，而路径规划是现自主导航的关键技术。目前，根据环境信息的不同，无人平台的路径规划分为已知环境下的全局路径规划和未知环境下的局部路径规划。
3.全局路径规划有利于求取全局最优路径，但其计算量相对较大，实时性较差，而且只能应用于静态环境中。局部路径规划则是根据传感数据构建的环境模型进行路径求取的，故有较好的实时性和应对动态环境的能力。此外，局部路径规划求取的路径较为光滑，利于无人平台的运动控制。
4.应用于智能车间场景下的无人平台既需要多平台之间的协作，又需要平台和人员之间的协作，因此路径冲突和动态障碍物问题不可避免。而现有的局部路径规划方法虽能够在一定程度上适应动态环境和规避动态障碍物，但应用在智能车间场景下时，安全避障问题仍然无法解决，故难以满足智能车间工业生产的要求。


技术实现要素：

5.本发明要解决的技术问题是怎样提高无人平台在动态环境中规避静态和动态障碍物的能力，获取更优的路径，提出了一种动态环境中基于时空栅格地图的局部路径规划方法及装置。
6.为解决上述技术问题，本发明所采用的技术方案是：
7.一种动态环境中基于时空栅格地图的局部路径规划方法，包括以下步骤：
8.步骤1：获取无人平台当前位置的二维静态局部栅格地图以及动态障碍物和无人平台的运动状态信息，构建时空栅格地图；
9.步骤2：在所述时空栅格地图上对所述无人平台进行路径规划，得到无人平台的时空路径；
10.步骤3：将所述无人平台的时空路径向x
‑
y平面投影得到无人平台的二维规划路径并输出。
11.进一步地，步骤1中所述构建时空栅格地图的方法是：
12.步骤1.1：将所述动态障碍物的初始位置添加到二维静态局部栅格地图中；
13.步骤1.2：根据动态障碍物的运动状态推算动态障碍物的运动轨迹；
14.步骤1.3：根据所述动态障碍物的运动轨迹改变动态障碍物在静态二维栅格地图中的位置；
15.步骤1.4：按照时间顺序依次堆叠不同时刻的静态二维栅格地图，建立时空栅格地
图。
16.进一步地，步骤2中在所述时空栅格地图上对所述无人平台进行路径规划，得到无人平台的时空路径的方法是：
17.步骤2.1：根据无人平台的运动状态信息确定所述无人平台的速度动态窗口；
18.步骤2.2：根据无人平台的线速度分辨率dv和角速度分辨率dω对所述动态窗口进行遍历采样，推算出无人平台处于不同运动状态下的m条轨迹；
19.步骤2.3：判断所述m条轨迹是否满足碰撞制动条件；
20.步骤2.4：对满足碰撞制动条件要求的n条轨迹进行评价，n≤m；
21.步骤2.5：将评价值最高的轨迹作为最优路径输出，并判断此路径是否到达目标点。
22.进一步地，步骤2.1中根据无人平台的运动状态确定所述无人平台的速度动态窗口的方法是：
[0023][0024]
v
a
为无人平台当前线速度，为线加速度，ω
a
为无人平台当前角速度，为角加速度，dt为时间间隔，v
min
和v
max
为无人平台的最小线速度和最大线速度，ω
min
和ω
max
为无人平台的最小角速度和最大角速度。
[0025]
进一步地，步骤2.4中对满足碰撞制动条件要求的n条轨迹进行评价的方法是：
[0026]
评价函数为：
[0027]
g(v，ω，t
ini
)＝σ
·
head(v，ω) β
·
dist(v，ω，t
ini
) γ
·
velocity(v，ω)
[0028]
其中，v为采样线速度，ω为采样角速度，t
ini
为采样初始时间，σ、β和γ为评价系数，head(v，ω)为方位角评价函数，dist(v，ω，t
ini
)为障碍物距离评价函数，velocity(v，ω)为速度评价函数，velocity(v，ω)＝v；
[0029]
方位角评价函数：
[0030]
head(v，ω)＝180
‑
θ
target
(v，ω)＝180
‑
|θ
goal
(v，ω)
‑
θ(v，ω)|；
[0031]
θ
target
(v，ω)为无人平台在(v，ω)下朝向与目标点之间夹角，θ
goal
(v，ω)为目标点相对于无人平台的方位角，θ(v，ω)为无人平台在(v，ω)下无人平台的姿态角；
[0032]
障碍物距离评价函数：
[0033]
dist(v，ω，t
ini
)＝min(pdist(obs(t)，traj(v，ω，t))|t＝t
ini
：dt：t
ini
δt)
‑
r；
[0034]
obs(t)为时空栅格地图中不同时刻t对应的障碍物位置，traj(v,ω,t)为时空栅格地图中无人平台的轨迹点，pdist()为两点的欧拉距离，dt为轨迹点时间间隔，δt为整条轨迹的时长，r为障碍物膨胀半径。
[0035]
进一步地，在对每条轨迹进行评价前，各项评价指标加权之前对每项评价指标进行归一化处理。
[0036]
本发明还提供了一种动态环境中基于时空栅格地图的局部路径规划装置，其特征在于，包括以下模块：
[0037]
时空地图构建模块：用于获取无人平台当前位置的二维静态局部栅格地图以及动态障碍物和无人平台的运动状态信息，构建时空栅格地图；
[0038]
时空路径获取模块：用于在所述时空栅格地图上对所述无人平台进行路径规划，得到无人平台的时空路径；
[0039]
路径规划输出模块：用于将所述无人平台的时空路径向x
‑
y平面投影得到无人平台的二维规划路径并输出。
[0040]
采用上述技术方案，本发明具有如下有益效果：
[0041]
本发明提供的一种动态环境中基于时空栅格地图的局部路径规划方法及装置，基于静态局部栅格地图和动态障碍物运动状态建立的时空栅格地图，能够统一整合局部地图中的静态障碍物和动态障碍物，将二维动态环境转化为三维静态环境，降低了动态环境中路径规划的复杂性。同时能够充分利用动态障碍物的运动信息，有效避免了动态障碍物对路径规划的不利影响。
附图说明
[0042]
图1为本发明的整体流程框图；
[0043]
图2为本发明的静态栅格地图；
[0044]
图3为本发明的时空栅格地图；
[0045]
图4为本发明的路径规划方法实现方案；
[0046]
图5为本发明的轨迹递推模型；
[0047]
图6为本发明的路径规划过程；
[0048]
图7为本发明的由时空路径投影为二维空间路径；
[0049]
图8为本发明的路径规划过程中速度变化。
具体实施方式
[0050]
下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0051]
图1至图8示出了本发明一种动态环境中基于时空栅格地图的局部路径规划方法的一种具体实施例，如图1所示，包括以下步骤：
[0052]
步骤1：获取无人平台当前位置的二维静态局部栅格地图以及动态障碍物和无人平台的运动状态信息，构建时空栅格地图。
[0053]
本实施例中，所述构建时空栅格地图的方法是：
[0054]
步骤1.1：将所述动态障碍物的初始位置添加到二维静态局部栅格地图中；
[0055]
步骤1.2：根据动态障碍物的运动状态推算动态障碍物的运动轨迹；
[0056]
步骤1.3：根据所述动态障碍物的运动轨迹改变动态障碍物在静态二维栅格地图中的位置；
[0057]
步骤1.4按照时间顺序依次堆叠不同时刻的静态二维栅格地图，建立时空栅格地图。
[0058]
本实施例中的静态局部栅格地图如图2所示，地图尺寸为20
×
20，其中含有尺寸为4
×
5的静态障碍物。动态障碍物的尺寸为1
×
1，起点为(12，1)，运动线速度v＝0.5，角速度ω＝0。构建的时空栅格地图如图3所示，将二维栅格地图按照时间顺序进行堆叠，地图时间
分辨率map_dt＝1s。
[0059]
步骤2：在所述时空栅格地图上对所述无人平台进行路径规划，得到无人平台的时空路径，如图4所示；
[0060]
步骤2.1：根据无人平台的运动状态确定所述无人平台的速度动态窗口。本实施例中确定所述无人平台的速度动态窗口的方法是：
[0061][0062]
本实施例中，v
a
为无人平台当前线速度，为线加速度，ω
a
为无人平台当前角速度，为角加速度，无人平台最大线速度v
max
＝4，最大角速度ω
max
＝0.35rad/s，最大线加速度最大角加速度时间间隔dt＝0.1s。
[0063][0064]
步骤2.2：根据无人平台的线速度分辨率dv和角速度分辨率dω对所述动态窗口进行遍历采样，推算出无人平台处于不同运动状态下的m条轨迹。本实施例中，无人平台速度分辨率dv＝0.01，角速度分辨率dω＝0.0175rad/s。
[0065]
无人平台的轨迹推算模型如图5所示，无人平台的轨迹由多段圆弧组成，一对线速度和角速度(v，ω)就代表了一段圆弧轨迹：
[0066][0067]
当轨迹预测的时间间隔dt＝0.1s时，轨迹推算公式为：
[0068][0069]
步骤2.3：判断所述m条轨迹是否满足碰撞制动条件；
[0070]
本实施例中，无人平台的制动加速度本实施例中，无人平台的制动加速度
[0071]
步骤2.4：对满足碰撞制动条件要求的n条轨迹进行评价，n≤m；
[0072]
评价函数为：
[0073]
g(v，ω，t
ini
)＝σ
·
head(v，ω) β
·
dist(v，ω，t
ini
) γ
·
velocity(v，ω)
[0074]
其中v为采样线速度，ω为采样角速度，t
ini
为采样初始时间，σ、β和γ为评价系数，head(v，ω)为方位角评价函数，dist(v，ω，t
ini
)为障碍物距离评价函数，velocity(v，ω)为速度评价函数，velocity(v，ω)＝v；
[0075]
方位角评价函数：
[0076]
head(v，ω)＝180
‑
θ
target
(v，ω)＝180
‑
|θ
goal
(v，ω)
‑
θ(v，ω)|；
[0077]
θ
target
(v，ω)为无人平台在(v，ω)下朝向与目标点之间夹角，θ
goal
(v，ω)为目标点相对于无人平台的方位角，θ(v，ω)为无人平台在(v，ω)下无人平台的姿态角；
[0078]
障碍物距离评价函数：
[0079]
dist(v，ω，t
ini
)＝min(pdist(obs(t)，traj(v，ω，t))|t＝t
ini
：dt：t
ini
δt)
‑
r；
[0080]
obs(t)为时空栅格地图中不同时刻t对应的障碍物位置，traj(v，ω，t)为时空栅格地图中无人平台的轨迹点，pdist()为两点的欧拉距离，dt为轨迹点时间间隔，δt为整条轨迹的时长，r为障碍物膨胀半径。本实施例中，膨胀半径是指为避免无人平台撞上障碍物，将障碍物沿垂直切线方向向外膨胀一定距离，并将此部分也看作障碍物。
[0081]
本实施例中，每条轨迹的各项评价指标加权之前需对每项指标进行归一化处理，以防止单项指标太占优势。归一化方式为每一项除以每一项的总和：
[0082][0083][0084][0085]
其中，head(i)、dist(i)和velocity(i)分别为同一动态窗口内第i条轨迹的方位角评分、障碍物距离评分和速度评分，nor_head(i)、nor_dist(i)和nor_velocity(i)分别为同一动态窗口内第i条轨迹的归一化方位角评分、归一化障碍物距离评分和归一化速度评分。
[0086]
评价函数各系数分别取σ＝0.045，β＝0.1，γ＝0.1，所以评价函数为：
[0087]
g(v，ω，t
ini
)＝0.045
·
nor
head(v，ω)
0.1
·
nor
dist
(v，ω，t
ini
) 0.1
·
nor
velocity
(v，ω)
[0088]
步骤2.5：将评价值最高的轨迹作为最优路径输出，并判断此路径是否到达目标点。
[0089]
本实施例中，判断路径是否到达目标点的距离阈值l＝0.25，并通过使用评价函数，选择评价值最高的轨迹作为最优路径，提高了动态环境中路径规划的成功率，获得了更为优秀的动态避障路径。
[0090]
图6为时空路径规划过程，黑色栅格为障碍物，黑色星号为目标点，灰色曲线为当前动态窗口所有采样速度对应的轨迹，黑色曲线为已规划的最优路径。本发明所述的时空路径除了能够观察到无人平台的避障路径，还可通过路径t方向的斜率直观判断无人平台的速度变化(斜率越大表示无人平台速度越慢)，便于分析无人平台的避障状态。
[0091]
步骤3：将所述无人平台的时空路径向x
‑
y平面投影得到无人平台的二维规划路径并输出。
[0092]
图7为本实施例中的时空路径和二维空间路径，由图7可见，此路径规划方法能在到达目标点的基础上较为优秀地避开所有动态障碍物和静态障碍物。图8为路径规划过程
中的速度变化过程，实线为无人平台线速度(v)，虚线为无人平台角速度(ω)，其中线速度有两次较为明显的减速，第一次减速在2s～5s时间段内，此时受到动态障碍物的影响，无人平台主动降低速度以避开动态障碍物，从图7中也可以看出，此段时间内时空路径的t方向斜率也是在增加的，与图8减速过程相吻合；第二次减速在15s～20s时间段内，当靠近目标点时，无人平台降低速度以准确地到达目标点位置，同样的，图7中时空路径此时间段内t方向斜率也是在增加的。
[0093]
本发明还提供了一种动态环境中基于时空栅格地图的局部路径规划装置，包括以下模块：
[0094]
时空地图构建模块：用于获取无人平台当前位置的二维静态局部栅格地图以及动态障碍物和无人平台的运动状态信息，构建时空栅格地图；
[0095]
时空路径获取模块：用于在所述时空栅格地图上对所述无人平台进行路径规划，得到无人平台的时空路径；
[0096]
路径规划输出模块：用于将所述无人平台的时空路径向x
‑
y平面投影得到无人平台的二维规划路径并输出。
[0097]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。