基于A*算法与改进最小化snap的无人飞行器路径规划方法技术

本发明专利技术为基于A*算法与改进最小化snap的无人飞行器路径规划方法，首先通过A*算法进行路径点搜索，得到初始轨迹；然后，利用最小化snap对A*算法得到的初始轨迹进行优化，得到一次优化轨迹；最后，逐一判断除起始点、目标点以外的所有路径点是否为候选冗余路径点，再判断每个候选冗余路径点是否为冗余路径点，若是冗余路径点则应删除，以此对一次优化轨迹进行冗余路径点修剪，对删除冗余路径点的轨迹重新添加约束条件，通过二次规划求解目标函数，完成整个路径规划。A*算法与最小化snap结合得到的轨迹尽管是连续、光滑的，但是轨迹较长，本发明专利技术通过修剪冗余路径点使轨迹更短，同时能够有效避障。避障。避障。

【技术实现步骤摘要】
基于A＊算法与改进最小化snap的无人飞行器路径规划方法


[0001]本专利技术属于无人飞行器运动规划
，涉及一种基于A*算法与改进最小化snap的无人飞行器路径规划方法。
技术背景
[0002]随着自主空中机器人技术的发展，微型无人飞行器，特别是四旋翼无人飞行器因其良好的机动性能而被广泛运用。微型无人飞行器通过搭载各种功能设备完成巡检、物资配送、救援等任务，运动规划是无人飞行器实现自主飞行的重要基础，运动规划包括前端路径规划和后端轨迹优化，其目的是寻找一条从起始点到目标点的最优路径。常见的运动规划算法主要包括图搜索算法、空间采样算法、曲线插值拟合算法和仿生智能算法。图搜索算法中的A*算法在迭代过程中利用对目标的启发式信息，可以在有障碍物的环境中规划出一条全局最优路径，但因其分辨率最优的特点，得到的路径很大程度上依赖于栅格地图的分辨率；此外，A*算法对高维信息有着很弱的处理能力，无法满足无人飞行器的动力学要求，所以引入最小化snap方法对A*算法得到的全局最优路径进行参数化处理，得到优化轨迹。
[0003]文献《移动机器人最小化snap与A*的联合轨迹优化》(韩晓微，刘宏宇，石泽亮，等.移动机器人最小化snap与A*的联合轨迹优化[J].沈阳大学学报(自然科学版)，2021，33(4)：321
‑
328.)针对A*算法所规划路径转角大、折线多且平滑性差的问题，提出一种基于最小化snap改进的A*轨迹规划方法。该方法得到的轨迹虽然是连续、光滑的，但是并不是最短轨迹，存在冗余路径点，导致飞行效率较低。
[0004]针对上述问题，本专利技术提出一种基于A*算法与改进最小化snap的无人飞行器路径规划方法，通过修剪冗余路径点，缩短轨迹长度。

技术实现思路

[0005]针对现有技术的不足，本专利技术拟解决的技术问题是，提出一种基于A*算法与改进最小化snap的无人飞行器路径规划方法。
[0006]本专利技术解决所述技术问题采用的技术方案如下：
[0007]1.一种基于A*算法与改进最小化snap的无人飞行器路径规划方法，其特征在于，该方法包括以下步骤：
[0008]第一步、根据环境地图构建栅格地图，利用A*算法进行路径点搜索，得到初始轨迹；
[0009]第二步、利用最小化snap对初始轨迹进行优化，得到一次优化轨迹；
[0010]最小化snap优化的目标函数为：
[0011][0012]式中，f
(4)
(t)表示第j段轨迹的加速度的二阶导数，T
j
‑1、T
j
分别表示第j段轨迹的起始时刻和截止时刻，t表示时刻；
[0013]最小化snap优化的约束条件为：
[0014][0015][0016]式中，分别表示第j、j+1段轨迹在T
j
时刻的k阶导数；式(4)表示第j段轨迹在T
j
时刻的k阶导数需要满足的约束；式(5)表示第j段轨迹在T
j
时刻的k阶导数与第j+1段轨迹在T
j
时刻的k阶导数相等；k取1、2、3、4表示第j段轨迹最后一个路径点与第j+1段轨迹第一个路径点的位置、速度、加速度以及加速度变化率分别对应相等；
[0017]第三步、对一次优化轨迹进行冗余路径点修剪，对轨迹重新添加约束条件，通过二次规划求解目标函数，得到二次优化轨迹；
[0018]判断除起始点、目标点以外的各个路径点是否为候选冗余路径点，若满足式(6)则为候选冗余路径节点，否则为正常路径点；
[0019]s
‑
(l*1.05)＞0
ꢀꢀꢀꢀ
(6)
[0020]式中，s＝s1+s2表示与待判断路径点相连的前后两段轨迹长度之和，s1表示待判断路径点与前一个路径点之间的轨迹长度，s2表示待判断路径点与后一个路径点之间的轨迹长度，l表示位于待判断路径点前、后两个路径点的欧式距离；
[0021]判断各个候选冗余路径点是否为冗余路径点，若为冗余路径点则删除；对于任意候选冗余路径点，若该候选冗余路径点至其后一路径点的连线方向为左上方，则判断该候选冗余路径点正左侧或正上方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；若该候选冗余路径点至其后一路径点的连线方向为右上方，则判断该候选冗余路径点正右侧或正上方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；若该候选冗余路径点至其后一路径点的连线方向为左下方，则判断该候选冗余路径点正左侧或正下方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；若该候选冗余路径点至其后一路径点的连线方向为右下方，则判断该候选冗余路径点正右侧或正下方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；
[0022]对于任意冗余路径点，连接该冗余路径点相邻的前、后两个路径点，生成飞行走廊的基线；以基线为基准向左、右两侧扩张飞行走廊，飞行走廊的左、右侧边界与邻近障碍物之间的间距d应满足下式：
[0023][0024]式中，w表示无人飞行器的宽度；
[0025]飞行走廊的边界约束条件为：
[0026]f
j
(T
j
)≤b
j
ꢀꢀꢀꢀ
(8)
[0027]b
j
＝x
obs
+d
ꢀꢀꢀꢀ
(9)
[0028]式中，f
j
(T
j
)表示第j段轨迹在T
j
时刻的位置，b
j
表示第j段轨迹飞行走廊的边界位置，x
obs
表示与飞行走廊邻近的障碍物的横坐标；
[0029]通过二次规划，根据式(5)、(8)的约束条件对式(3)的目标函数进行求解，得到二次优化轨迹，完成路径规划。
[0030]与现有技术相比，本专利技术的有益效果是：
[0031]A*算法与最小化snap结合得到的轨迹尽管是连续、光滑的，但是A*算法的路径搜索严重依赖于栅格地图的分辨率，故存在局部最优问题，得到的轨迹存在冗余路径点，因此轨迹较长。本专利技术针对此问题进行改进，对最小化snap优化得到的轨迹进行冗余路径点修剪，修剪冗余路径点后，冗余路径点对应的轨迹缺少约束，因此为缺少约束的区域添加飞行走廊，生成约束条件，再次通过二次规划对目标函数进行求解，得到的二次优化轨迹更短，同时能够有效避障。只针对缺少约束的区域添加飞行走廊，而不是对整个轨迹添加飞行走廊，减少了计算量。
附图说明
[0032]图1为栅格地图的示意图；
[0033]图2为一次优化轨迹的示意图；
[0034]图3为候选冗余路径点的判断原理图；
[0035]图本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于A*算法与改进最小化snap的无人飞行器路径规划方法，其特征在于，该方法包括以下步骤：第一步、根据环境地图构建栅格地图，利用A*算法进行路径点搜索，得到初始轨迹；第二步、利用最小化snap对初始轨迹进行优化，得到一次优化轨迹；最小化snap优化的目标函数为：式中，f
(4)
(t)表示第j段轨迹的加速度的二阶导数，T
j
‑1、T
j
分别表示第j段轨迹的起始时刻和截止时刻，t表示时刻；最小化snap优化的约束条件为：最小化snap优化的约束条件为：式中，f
j(k)
(T
j
)、分别表示第j、j+1段轨迹在T
j
时刻的k阶导数；式(4)表示第j段轨迹在T
j
时刻的k阶导数需要满足的约束；式(5)表示第j段轨迹在T
j
时刻的k阶导数与第j+1段轨迹在T
j
时刻的k阶导数相等；k取1、2、3、4表示第j段轨迹最后一个路径点与第j+1段轨迹第一个路径点的位置、速度、加速度以及加速度变化率分别对应相等；第三步、对一次优化轨迹进行冗余路径点修剪，对轨迹重新添加约束条件，通过二次规划求解目标函数，得到二次优化轨迹；判断除起始点、目标点以外的各个路径点是否为候选冗余路径点，若满足式(6)则为候选冗余路径节点，否则为正常路径点；s
‑
(l*1.05)>0
ꢀꢀꢀꢀꢀꢀꢀꢀ
(6)式中，s＝s1+s2表示与待判断路径点相连的前后两段轨迹长度之和，s1表示待判断路径点与前一个路径点之间的轨迹长度，s2表示待判断路径点与后一个路径点之间的轨迹长度，l表示位于待判断路径点前、后两个路径点的欧式距离；判断各个候选冗余路径点是否为冗余路径点，若为冗余路径点则删除；对于任意候选冗余...

【专利技术属性】
技术研发人员：顾军华，左宗霖，郑晓园，刘洪普，张亚娟，
申请(专利权)人：河北工业大学，
类型：发明
国别省市：