路径优化方法及装置、存储介质及设备制造方法及图纸

本公开涉及一种路径优化方法及装置、电子设备及存储介质，所述方法包括：对预先规划的路径的点序列进行降采样，得到采样点集，并计算采样点集中每一采样点与障碍物之间的最小距离；根据每一采样点与障碍物之间的最小距离以及每一采样点与相邻采样点之间的距离，更新采样点的数量和位置；对于更新后的采样点，确定每一采样点更新后与更新前位置之间的距离；响应于每一采样点更新后与更新前位置之间的距离小于预设阈值，将更新后的采样点作为确定后的采样点；对确定后的采样点进行插值处理，得到优化后的路径，通过降采样减少路径优化的运算量，满足低运算平台的要求，并考虑到障碍物，确保路径的平滑性且能够避免撞上障碍物。确保路径的平滑性且能够避免撞上障碍物。确保路径的平滑性且能够避免撞上障碍物。

【技术实现步骤摘要】
路径优化方法及装置、存储介质及设备


[0001]本公开涉及机器人导航
，尤其涉及一种路径优化方法及装置、存储介质及设备。

技术介绍

[0002]机器人导航框架分为两部分，一部分是路径规划，另一部分是路径跟踪。
[0003]对于常用规划算法(如Dijkstra和A*等)，使用的是栅格地图作为基础，在栅格地图上搜索出满足代价最小的路径。由于栅格地图的分辨率不能太高，太高会导致运算量急剧上升，而分辨率太低则会导致路径不够精确。因此，在权衡运算量与精度的基础上，会使用一定的分辨率的栅格地图进行路径规划，但这就导致生成的路径往往不是平滑的或者最短的，存在一定的弯曲，特别是在狭窄的过道处。
[0004]而弯曲的路径对于路径跟踪来说，是一个不好的输入。路径跟踪的评价指标是控制机器人走的路径是否贴合给定的路径。路径跟踪算法越好，那么会控制机器人越贴合给定的路径。如果给定路径存在弯曲的情况，机器人走出来路径也是弯曲的，如果路径弯曲程度过大，可能会超出机器人的控制极限范围，导致机器人急停或者漂移的情况，用户体验会大大下降。另外，如果在空旷场景中规划的路径也是弯曲的，会导致机器人也走弯曲的路线，给用户一种机器人很不智能的感觉。
[0005]那能否降低路径跟踪的精度，从而满足机器人行走平顺的要求？实际上，路径跟踪能够对路径的曲折部分进行过滤，那么也必然导致路径跟踪的贴合程度降低，从而增加机器人撞上障碍物的风险。
[0006]目前大部分机器人在规划路径后，都把路径直接输入到路径跟踪去，而不进行路径优化的处理，通过调整路径跟踪的参数(如前瞻点等)，让机器人不跟随路径弯弯曲曲的部分，这样必然导致路径跟踪的精度下降。而且在路径规划的层面一般不考虑机器的运动特性，特定情况下会产生一些弯曲、震荡的路径，导致机器人运动剧烈。
[0007]另外，一些路径优化(路径平滑)的方法是基于样条曲线对路径进行平滑，也有佛洛依德路径平滑算法对路径进行平滑的。其中，基于样条曲线的优化方法中，样条是一种分段的低阶多项式逼近函数，可应用于具有不同非线性度或者存在多个极值点的函数，平滑后的路径具有n阶连续的特点。基于佛洛依德路径平滑算法的路径平滑方法原理简单，目的是去掉相邻共线的点和去掉多余拐弯的点。
[0008]然而，对于样条曲线的平滑方法，优化过程中只考虑了位置点的位置，没有考虑障碍物的位置，容易造成优化出来的路径撞上障碍物，或者远离障碍物不足；基于佛洛依德路径平滑算法的路径平滑方法，往往需要将整条路径的所有路径点输入到优化算法去进行优化，导致运算量比较大，不适合使用在低运算量的平台上进行。

技术实现思路

[0009]为了解决上述技术问题或者至少部分地解决上述技术问题，本公开的实施例提供
了一种路径优化方法及装置、存储介质及设备。
[0010]第一方面，本公开的实施例提供了一种方法，包括：
[0011]对预先规划的路径的点序列进行降采样，得到采样点集，并计算采样点集中每一采样点与障碍物之间的最小距离；
[0012]根据每一采样点与障碍物之间的最小距离以及每一采样点与相邻采样点之间的距离，更新采样点的数量和位置；
[0013]对于更新后的采样点，确定每一采样点更新后与更新前位置之间的距离；
[0014]响应于每一采样点更新后与更新前位置之间的距离小于预设阈值，将更新后的采样点作为确定后的采样点；
[0015]对确定后的采样点进行插值处理，得到优化后的路径。
[0016]在一种可能的实施方式中，所述方法还包括：
[0017]响应于更新后的采样点中，任意一个采样点更新后与更新前位置之间的距离大于或等于预设阈值，重新执行计算每一采样点与障碍物之间的最小距离的步骤，直到每一采样点更新后与更新前位置之间的距离小于预设阈值为止，其中，对于更新后的采样点中新增的采样点，更新后与更新前位置之间的距离为无穷大或者比预设阈值大。
[0018]在一种可能的实施方式中，通过以下表达式，计算采样点集中每一采样点与障碍物之间的最小距离：
[0019]d＝
‑
ln(cost/253)/a+r
[0020]其中，d为当前采样点与障碍物之间的最小距离，cost为当前采样点所在代价地图上栅格的代价值，a为衰减系数，r为用于路径跟踪的机器人的外切圆半径。
[0021]在一种可能的实施方式中，所述根据每一采样点与障碍物之间的最小距离以及每一采样点与相邻采样点之间的距离，更新采样点的数量和位置，包括：
[0022]根据当前采样点与障碍物之间的最小距离计算当前采样点的无碰撞空间范围大小；
[0023]基于当前采样点与相邻采样点之间的距离以及当前采样点和相邻采样点的无碰撞空间范围大小，修正采样点；
[0024]根据当前采样点与相邻采样点之间的距离以及当前采样点与障碍物之间的最小距离调整当前采样点的位置。
[0025]在一种可能的实施方式中，通过以下表达式，根据当前采样点与障碍物之间的最小距离计算当前采样点的无碰撞空间范围大小：
[0026]ρ＝d
‑
r＝
‑
ln(cost/253)/a
[0027]其中，ρ为当前采样点的无碰撞空间范围大小，d为当前采样点与障碍物之间的最小距离，r为用于路径跟踪的机器人的外切圆半径，cost为当前采样点所在代价地图上栅格的代价值，a为衰减系数。
[0028]在一种可能的实施方式中，所述基于当前采样点与相邻采样点之间的距离以及当前采样点和相邻采样点的无碰撞空间范围大小，修正采样点，包括：
[0029]在相邻两个采样点满足||b
i
‑
b
i+1
||≥(ρ
i
+ρ
i+1
)的条件下，在所述相邻两个采样点之间增加新的采样点，其中，所述新的采样点的位置在所述相邻两个采样点之间的中点；
[0030]在第i
‑
1个采样点与第i+1个采样点满足||b
i
‑1‑
b
i+1
||≤(ρ
i
‑1+ρ
i+1
)的条件下，删除
第i个采样点，其中，b
i
‑1为第i
‑
1个采样点的位置，b
i
为第i个采样点的位置，b
i+1
为第i+1个采样点的位置，ρ
i
‑1为第i
‑
1个采样点的无碰撞空间范围大小，ρ
i
为第i个采样点的无碰撞空间范围大小，ρ
i+1
为第i+1个采样点的无碰撞空间范围大小。
[0031]在一种可能的实施方式中，所述根据当前采样点与相邻采样点之间的距离以及当前采样点与障碍物之间的最小距离调整当前采样点的位置，包括：
[0032]步骤一，通过以下表达式，计算第i个采样点受到的相邻两个采样点的引力：本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种路径优化方法，其特征在于，所述方法包括：对预先规划的路径的点序列进行降采样，得到采样点集，并计算采样点集中每一采样点与障碍物之间的最小距离；根据每一采样点与障碍物之间的最小距离以及每一采样点与相邻采样点之间的距离，更新采样点的数量和位置；对于更新后的采样点，确定每一采样点更新后与更新前位置之间的距离；响应于每一采样点更新后与更新前位置之间的距离小于预设阈值，将更新后的采样点作为确定后的采样点；对确定后的采样点进行插值处理，得到优化后的路径。2.根据权利要求1所述的方法，其特征在于，所述方法还包括：响应于更新后的采样点中，任意一个采样点更新后与更新前位置之间的距离大于或等于预设阈值，重新执行计算每一采样点与障碍物之间的最小距离的步骤，直到每一采样点更新后与更新前位置之间的距离小于预设阈值为止，其中，对于更新后的采样点中新增的采样点，更新后与更新前位置之间的距离为无穷大或者比预设阈值大。3.根据权利要求1所述的方法，其特征在于，通过以下表达式，计算采样点集中每一采样点与障碍物之间的最小距离：d＝
‑
ln(cost/253)/a+r其中，d为当前采样点与障碍物之间的最小距离，cost为当前采样点所在代价地图上栅格的代价值，a为衰减系数，r为用于路径跟踪的机器人的外切圆半径。4.根据权利要求1所述的方法，其特征在于，所述根据每一采样点与障碍物之间的最小距离以及每一采样点与相邻采样点之间的距离，更新采样点的数量和位置，包括：根据当前采样点与障碍物之间的最小距离计算当前采样点的无碰撞空间范围大小；基于当前采样点与相邻采样点之间的距离以及当前采样点和相邻采样点的无碰撞空间范围大小，修正采样点；根据当前采样点与相邻采样点之间的距离以及当前采样点与障碍物之间的最小距离调整当前采样点的位置。5.根据权利要求4所述的方法，其特征在于，通过以下表达式，根据当前采样点与障碍物之间的最小距离计算当前采样点的无碰撞空间范围大小：ρ＝d
‑
r＝
‑
ln(cost/253)/a其中，ρ为当前采样点的无碰撞空间范围大小，d为当前采样点与障碍物之间的最小距离，r为用于路径跟踪的机器人的外切圆半径，cost为当前采样点所在代价地图上栅格的代价值，a为衰减系数。6.根据权利要求4所述的方法，其特征在于，所述基于当前采样点与相邻采样点之间的距离以及当前采样点和相邻采样点的无碰撞空间范围大小，修正采样点，包括：在相邻两个采样点满足||b
i
‑
b
i+1
||≥(ρ
i
+ρ
i+1
)的条件下，在所述相邻两个采样点之间增加新的采样点，其中，所述新的采样点的位置在所述相邻两个采样点之间的中点；在第i
‑
1个采样点与第i+1个采样点满足||b
i
‑1‑
b
i+1
||≤(ρ
i
‑1+ρ
i+1
)的条件下，删除第i个采样点，其中，b
i
‑1为第i
‑
1个采样点的位置，b
i
为第i个采样点的位置，b
i+1
为第i+1个采样点的位置，ρ
i
‑1为第i
‑
1个采样点的无碰撞空间范围大小，ρ
i
为第i个采样点的无碰撞空间范
围大小，ρ
i+1

【专利技术属性】
技术研发人员：王运志，
申请(专利权)人：重庆中科汽车软件创新中心，
类型：发明
国别省市：