一种扭曲空间的避障路径规划方法技术

本发明专利技术公开了一种扭曲空间的避障路径规划方法，其包括如下步骤：建立构形空间的障碍物地图；将构形空间的障碍物进行降维坍缩，生成无障碍物的扭曲空间；在扭曲空间中用直线连接起点与终点，还原回构形空间得到初始路径；对初始路径进行局部裁剪优化得到最终的优化路径。本发明专利技术能够快速生成一条从起点到终点的避障路径，计算效率明显优于A*等传统路径规划方法，同时路径长度较优，本发明专利技术将障碍物坍缩的方式能够高效处理各类复杂障碍物的情况，且适用于任意高维地图。适用于任意高维地图。适用于任意高维地图。

【技术实现步骤摘要】
一种扭曲空间的避障路径规划方法


[0001]本专利技术属于避障轨迹规划领域，具体涉及一种扭曲空间的避障路径规划方法。

技术介绍

[0002]机器人在特定环境执行任务时，需要规划出一条连接起始点与终止点之间的安全路径，以展开导航任务。传统的路径规划算法大多可以分为以下几类：图搜索方法、基于采样的方法和智能仿生方法。启发函数对于图搜索算法(如A*算法)的性能具有重要影响，在进行高维空间路径规划时，计算成本和数据存储空间会呈指数性增长，导致计算效率降低。基于采样的方法，如随机探索快速树(RRT)算法和概率路标方法等，是能够在高维地图上快速求解的随机方法，但算法收敛速度较慢，当工作环境中障碍物较多或通道狭窄时，计算效率会严重降低。智能仿生算法(如蚁群优化算法和遗传算法等)具有很强的鲁棒性，但也存在计算量大的局限性。
[0003]有不少基于传统路径规划算法的改进方法，例如，徐德刚、罗仔明等人公开的一种基于STAR算法优化的快速BI_RRT避障轨迹规划方法，专利号CN111678523A，其方法通过对双向生长树的生长规律进行设计，利用对生成的路径进行重选父节点以及重新焊接树的操作，使得最终路径趋近于最优路径，提升了避障轨迹规划的效率，但是在高维及复杂障碍物地图下，计算效率会大幅降低，且由于其随机性难以预期其计算成本。侯阿临、杨骐豪等人公开的一种基于改进蚁群算法的路径规划方法，专利号CN111982125A，该算法将改进的人工势场(APF)算法和蚁群算法相结合，降低了蚁群算法初始规划的盲目性，提高了算法的运行效率与全局最优性，但是该方法的计算量大，计算成本高。
[0004]因此，当前缺乏一种适用于高维地图与复杂障碍物地图，且计算效率高、鲁棒性强的路径规划算法。

技术实现思路

[0005]针对现有技术存在的缺陷，本专利技术的目的是提供一种扭曲空间的避障路径规划方法，将构形空间的障碍物降维坍缩，得到无障碍物的扭曲空间，在扭曲空间之内连接起始点与终点，将路径还原回构形空间后进行路径优化，最终快速获得构形空间下的避障路径。
[0006]为达到上述目的，本专利技术采用如下技术方案：
[0007]一种扭曲空间的避障路径规划方法，具体包括如下步骤：
[0008]S1.建立构形空间地图：基于机器人本身的运动学模型，获取构形空间的障碍物地图；
[0009]S2.生成扭曲空间：将构形空间中的所有障碍物块坍缩，保持边界节点不变，自由节点伴随障碍物块坍塌而发生扭曲，得到无障碍物的扭曲空间；
[0010]S3.生成初始路径：在扭曲空间中直接用直线连接起点与终点，得到扭曲空间中的路径，将扭曲空间的路径还原回构形空间，得到在构形空间的初始路径；
[0011]S4.局部路径裁剪优化：对构形空间的初始路径进行局部裁剪优化，得到最终的优
化路径。
[0012]进一步地，所述步骤S1中，所述构形空间是基于机器人运动学模型，建立n自由度机器人从任务空间到n维构形空间的投影关系，对于机器人构形空间中所有网格单元，计算机器人对应真实位姿在任务空间中是否与障碍物发生碰撞或者产生自碰撞，如果发生碰撞，则机器人构形空间中对应网格单元构成构形空间中的障碍物块，从而生成构形空间中的障碍物地图。
[0013]在本专利技术中进行如下定义：构形空间中的障碍物分为游离障碍物TP，即障碍物所含的网格单元与边界网格单元无交集，和边界障碍物TB，即障碍物所含的网格单元与边界网格单元有交集，障碍物块总体用T＝{TP,TB}，来描述，其中TP
l
是第l个游离障碍物块，n
c
是游离障碍物块的总数，TB
m
是第m个边界障碍物块，n
d
是边界障碍物块的总数；属于障碍物块的网格节点集合用和表示，其中表示属于第l个游离障碍物块TP
l
的网格节点，表示属于第m个边界障碍物块TB
m
的网格节点；构形空间的边界节点集合用N
boun
表示；NB
m
为障碍物TB
m
中和N
boun
的交集；NP
m
为障碍物块TB
m
中不属于N
boun
的部分；构形空间整体的网格节点表达为自由节点N
free
表示不属于边界节点和障碍物节点的节点集合；构形空间中第i个网格节点的坐标为其中是节点i沿着j轴的坐标，节点i的n自由度空间索引为(i1，i2，...，i
j
，...，i
n
)，其中i
j
是节点i沿着j轴的索引。
[0014]进一步地，所述步骤S2中，所述构形空间中的任意节点i按照公式(1
‑
4)联立出一个线性矩阵等式，求解该矩阵等式、计算出扭曲空间中扭曲后的节点i的坐标，其坐标用表示；扭曲前后节点i的n自由度空间索引不变：
[0015][0016]所述公式(2)表示对自由节点N
free
进行扭曲，扭曲后的自由节点位于其周围节点的几何中心上；当i∈N
free
时，扭曲后节点i的坐标满足方程其中分别表示节点i沿第j根轴方向索引前一位与索引后一位的节点，即的n自由度空间索引为(i1，...，(i
j
‑
1)
j
，...，i
n
)，的n自由度空间索引(i1，...，(i
j
+1)
j
，...，i
n
)。
[0017]所述公式(3)表示将构形空间中的游离障碍物坍缩到自动选择的扭曲点，所有属于游离障碍物的节点都扭曲到扭曲点的坐标位置上；第l个游离障碍物TP
l
的扭曲点P
lT
的坐标是：
[0018][0019]其中在障碍物TP
l
外侧包裹该障碍物的周围节点集为为障碍物TP
l
周围节点的个数，表示扭曲后障碍物TP
l
周围节点的位置；对于有vd
i
＝P
lT
，表示节点i扭曲到P
lT
的坐标上，即节点i满足公式(3)。
[0020]所述公式(4)依据节点生长算法得到NB
m
与NP
m
中节点的对应关系，用此公式计算属于NP
m
的节点扭曲后的坐标。节点生长算法具体为：对于边界障碍物集合TB＝{TB
m
|m＝1,2,...,n
m
}中的任一边界障碍物TB
m
，其网格节点可分为NB
m
和NP
m
，将NB
m
中每个节点设置一个根节点编号，并设置父节点点集Node
parent
＝NB
m
。另设置临时父节点点集Node
parent_temp
为空；当NP本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种扭曲空间的避障路径规划方法，其特征在于，将构形空间的障碍物降维坍缩，得到无障碍物的扭曲空间，在扭曲空间之内连接起始点与终点，将路径还原回构形空间后进行路径优化，获得躲避障碍物的最短路径，具体包括如下步骤：S1.建立构形空间地图：基于机器人本身的运动学模型，获取构形空间的障碍物地图；S2.生成扭曲空间：将构形空间中的所有障碍物块坍缩，保持边界节点不变，自由节点伴随障碍物块坍塌而发生扭曲，得到无障碍物的扭曲空间；S3.生成初始路径：在扭曲空间中直接用直线连接起点与终点，得到扭曲空间中的路径，将扭曲空间的路径还原回构形空间，得到在构形空间的初始路径；S4.局部路径裁剪优化：对构形空间的初始路径进行局部裁剪优化，得到最终的优化路径。2.根据权利要求1所述的基于扭曲空间的避障路径规划方法，其特征在于，所述步骤S1中，所述构形空间是基于机器人运动学模型，建立n自由度机器人从任务空间到n维构形空间的投影关系；对于机器人构形空间中所有网格单元，计算机器人对应真实位姿在任务空间中是否与障碍物发生碰撞或者产生自碰撞，如果发生碰撞，则机器人构形空间中对应网格单元构成构形空间中的障碍物块，从而生成构形空间中的障碍物地图；采用如下定义：构形空间中的障碍物分为游离障碍物TP，即障碍物所含的网格单元与边界网格单元无交集，和边界障碍物TB，即障碍物所含的网格单元与边界网格单元有交集，障碍物块总体用T＝{TP,TB}，来描述，其中TP
l
是第l个游离障碍物块，n
c
是游离障碍物块的总数，TB
m
是第m个边界障碍物块，n
d
是边界障碍物块的总数；属于障碍物块的网格节点集合用和表示，其中表示属于第l个游离障碍物块TP
l
的网格节点，表示属于第m个边界障碍物块TB
m
的网格节点；构形空间的边界节点集合用N
boun
表示；NB
m
为障碍物TB
m
中和N
boun
的交集；NP
m
为障碍物块TB
m
中不属于N
boun
的部分；构形空间整体的网格节点表达为自由节点N
free
表示不属于边界节点和障碍物节点的节点集合；构形空间中第i个网格节点的坐标为其中是节点i沿着j轴的坐标，节点i的n自由度空间索引为(i1，i2，...，i
j
，...，i
n
)，其中i
j
是节点i沿着j轴的索引。3.根据权利要求1所述的基于扭曲空间的避障路径规划方法，其特征在于，所述步骤S2中，所述构形空间中的任意节点i按照公式(1
‑
4)联立出一个线性矩阵等式，求解该矩阵等式、计算出扭曲空间中扭曲后的节点i的坐标，其坐标用表示；扭曲前后节点i的n自由度空间索引不变：
4.根据权利要求3所述的基于扭曲空间的避障路径规划方法，其特征在于，所述公式(2)表示对自由节点N
free
进行扭曲，扭曲后的自由节点位于其周围节点的几何中心上；当i∈N
free
时，扭曲后节点i的坐标满足方程其中分别表示节点i沿j轴方向索引前一位与索引后一位的节点，即的n自由度空间索引为(i1，...，(i
j
‑
1)
j
，...，i
n
)，的n自由度空间索引(i1，...，(i
j
+1)
j
，...，i
n
)。5.根据权利要求3所述的基于扭曲空间的避障路径规划方法，其特征在于，所述公式(3)表示将构形空间中的游离障碍物坍缩到自动选择的扭曲点，所有属于游离障碍物的节点都扭曲到扭曲点的坐标位置上；第l个游离障碍物TP
l
的扭曲点P
lT
的坐标是：其中在障碍物TP
l
外侧包裹该障碍物的周围节点集为外侧包裹该障碍物的周围节点集为为障碍物TP
l
周围节点的个数，表示扭曲后障碍物TP
l
周围节点的位置；对于有vd
i
＝P
lT
，表示节点i扭曲到P
lT
的坐标上，即节点i满足公式(3)。6.根据权利要求3所述的基于扭曲空间的避障路径规划方法，其特征在于，依据节点生长算法得到NB
m
与NP
m
中节点的对应关系，用公式(4)计算属于NP
m
的节点扭曲后的坐标；节点生长算法具体为：对于边界障碍物集合TB＝{TB
m
|m＝1,2,...,n
m
}中的任一边界障碍物TB

【专利技术属性】
技术研发人员：史航，饶梦，周锐，仇文玮，刘晨宇，周济宸，
申请(专利权)人：上海大学，
类型：发明
国别省市：