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

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

Info

Publication number
CN116243724A
CN116243724A CN202310152583.9A CN202310152583A CN116243724A CN 116243724 A CN116243724 A CN 116243724A CN 202310152583 A CN202310152583 A CN 202310152583A CN 116243724 A CN116243724 A CN 116243724A
Authority
CN
China
Prior art keywords
path point
track
redundant path
point
candidate
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310152583.9A
Other languages
English (en)
Inventor
顾军华
左宗霖
郑晓园
刘洪普
张亚娟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hebei University of Technology
Original Assignee
Hebei University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hebei University of Technology filed Critical Hebei University of Technology
Priority to CN202310152583.9A priority Critical patent/CN116243724A/zh
Publication of CN116243724A publication Critical patent/CN116243724A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • G05D1/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

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

Description

基于A＊算法与改进最小化snap的无人飞行器路径规划方法
技术领域
本发明属于无人飞行器运动规划技术领域，涉及一种基于A*算法与改进最小化snap的无人飞行器路径规划方法。
技术背景
随着自主空中机器人技术的发展，微型无人飞行器，特别是四旋翼无人飞行器因其良好的机动性能而被广泛运用。微型无人飞行器通过搭载各种功能设备完成巡检、物资配送、救援等任务，运动规划是无人飞行器实现自主飞行的重要基础，运动规划包括前端路径规划和后端轨迹优化，其目的是寻找一条从起始点到目标点的最优路径。常见的运动规划算法主要包括图搜索算法、空间采样算法、曲线插值拟合算法和仿生智能算法。图搜索算法中的A*算法在迭代过程中利用对目标的启发式信息，可以在有障碍物的环境中规划出一条全局最优路径，但因其分辨率最优的特点，得到的路径很大程度上依赖于栅格地图的分辨率；此外，A*算法对高维信息有着很弱的处理能力，无法满足无人飞行器的动力学要求，所以引入最小化snap方法对A*算法得到的全局最优路径进行参数化处理，得到优化轨迹。
文献《移动机器人最小化snap与A*的联合轨迹优化》(韩晓微，刘宏宇，石泽亮，等.移动机器人最小化snap与A*的联合轨迹优化[J].沈阳大学学报(自然科学版)，2021，33(4)：321-328.)针对A*算法所规划路径转角大、折线多且平滑性差的问题，提出一种基于最小化snap改进的A*轨迹规划方法。该方法得到的轨迹虽然是连续、光滑的，但是并不是最短轨迹，存在冗余路径点，导致飞行效率较低。
针对上述问题，本发明提出一种基于A*算法与改进最小化snap的无人飞行器路径规划方法，通过修剪冗余路径点，缩短轨迹长度。
发明内容
针对现有技术的不足，本发明拟解决的技术问题是，提出一种基于A*算法与改进最小化snap的无人飞行器路径规划方法。
本发明解决所述技术问题采用的技术方案如下：
1.一种基于A*算法与改进最小化snap的无人飞行器路径规划方法，其特征在于，该方法包括以下步骤：
第一步、根据环境地图构建栅格地图，利用A*算法进行路径点搜索，得到初始轨迹；
第二步、利用最小化snap对初始轨迹进行优化，得到一次优化轨迹；
最小化snap优化的目标函数为：
Figure BDA0004091331990000011
式中，f(4)(t)表示第j段轨迹的加速度的二阶导数，Tj-1、Tj分别表示第j段轨迹的起始时刻和截止时刻，t表示时刻；
最小化snap优化的约束条件为：
Figure BDA0004091331990000021
Figure BDA0004091331990000022
式中，
Figure BDA0004091331990000023
分别表示第j、j+1段轨迹在Tj时刻的k阶导数；式(4)表示第j段轨迹在Tj时刻的k阶导数需要满足的约束；式(5)表示第j段轨迹在Tj时刻的k阶导数与第j+1段轨迹在Tj时刻的k阶导数相等；k取1、2、3、4表示第j段轨迹最后一个路径点与第j+1段轨迹第一个路径点的位置、速度、加速度以及加速度变化率分别对应相等；
第三步、对一次优化轨迹进行冗余路径点修剪，对轨迹重新添加约束条件，通过二次规划求解目标函数，得到二次优化轨迹；
判断除起始点、目标点以外的各个路径点是否为候选冗余路径点，若满足式(6)则为候选冗余路径节点，否则为正常路径点；
s-(l*1.05)＞0 (6)
式中，s＝s1+s2表示与待判断路径点相连的前后两段轨迹长度之和，s1表示待判断路径点与前一个路径点之间的轨迹长度，s2表示待判断路径点与后一个路径点之间的轨迹长度，l表示位于待判断路径点前、后两个路径点的欧式距离；
判断各个候选冗余路径点是否为冗余路径点，若为冗余路径点则删除；对于任意候选冗余路径点，若该候选冗余路径点至其后一路径点的连线方向为左上方，则判断该候选冗余路径点正左侧或正上方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；若该候选冗余路径点至其后一路径点的连线方向为右上方，则判断该候选冗余路径点正右侧或正上方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；若该候选冗余路径点至其后一路径点的连线方向为左下方，则判断该候选冗余路径点正左侧或正下方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；若该候选冗余路径点至其后一路径点的连线方向为右下方，则判断该候选冗余路径点正右侧或正下方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；
对于任意冗余路径点，连接该冗余路径点相邻的前、后两个路径点，生成飞行走廊的基线；以基线为基准向左、右两侧扩张飞行走廊，飞行走廊的左、右侧边界与邻近障碍物之间的间距d应满足下式：
Figure BDA0004091331990000024
式中，w表示无人飞行器的宽度；
飞行走廊的边界约束条件为：
fj(Tj)≤bj (8)
bj＝xobs+d (9)
式中，fj(Tj)表示第j段轨迹在Tj时刻的位置，bj表示第j段轨迹飞行走廊的边界位置，xobs表示与飞行走廊邻近的障碍物的横坐标；
通过二次规划，根据式(5)、(8)的约束条件对式(3)的目标函数进行求解，得到二次优化轨迹，完成路径规划。
与现有技术相比，本发明的有益效果是：
A*算法与最小化snap结合得到的轨迹尽管是连续、光滑的，但是A*算法的路径搜索严重依赖于栅格地图的分辨率，故存在局部最优问题，得到的轨迹存在冗余路径点，因此轨迹较长。本发明针对此问题进行改进，对最小化snap优化得到的轨迹进行冗余路径点修剪，修剪冗余路径点后，冗余路径点对应的轨迹缺少约束，因此为缺少约束的区域添加飞行走廊，生成约束条件，再次通过二次规划对目标函数进行求解，得到的二次优化轨迹更短，同时能够有效避障。只针对缺少约束的区域添加飞行走廊，而不是对整个轨迹添加飞行走廊，减少了计算量。
附图说明
图1为栅格地图的示意图；
图2为一次优化轨迹的示意图；
图3为候选冗余路径点的判断原理图；
图4为冗余路径点的判断流程图；
图5为两次优化轨迹的对比图。
具体实施方式
下面结合附图和具体实施例对本发明的技术方案进行详细描述，但并不以此限定本申请的保护范围。
本发明为一种基于A*算法与改进最小化snap的无人飞行器路径规划方法，包括以下步骤：
第一步、根据无人飞行器运动的环境地图构建栅格地图，用于前端路径规划；栅格地图参见图1，图中包含起始点、目标点、障碍物以及可行区域；
根据栅格地图，利用A*算法进行路径点搜索，得到初始轨迹；A*算法通过构造代价函数建立搜索准则，代价函数的一般形式为：
f(m)＝g(m)+h(m) (1)
式中，f(m)为从起始点经过中间节点m到达目标点的最小估计代价，g(m)为从起始点到中间节点m已产生的累计代价，h(m)为从中间节点m到目标点还需产生的累计代价；
第二步、利用最小化snap对A*算法得到的初始轨迹进行优化，得到一次优化轨迹；
由于A*算法搜索得到的路径点较稀疏，容易造成轨迹不平滑，为了更好地控制无人飞行器运动，需要将稀疏的路径点变为平滑的轨迹；使用时间对轨迹进行参数化，以获得轨迹的高阶信息，用多段多项式表示轨迹，则一段轨迹的参数化形式为：
Figure BDA0004091331990000031
式中，pi表示第i项的系数，t表示时刻，n＝2k-1表示多项式的项数，k表示轨迹的求导阶数；
对于第j段轨迹，最小化snap优化的目标函数为：
Figure BDA0004091331990000041
式中，f(4)(t)表示第j段轨迹的加速度的二阶导数，Tj-1、Tj分别表示第j段轨迹的起始时刻和截止时刻；
最小化snap优化的约束条件为：
Figure BDA0004091331990000042
Figure BDA0004091331990000043
式中，
Figure BDA0004091331990000044
分别表示第j、j+1段轨迹在Tj时刻的k阶导数，式(4)为导数约束，表示第j段轨迹在Tj时刻的k阶导数需要满足的约束，式(5)为连续性约束，表示第j段轨迹在Tj时刻的k阶导数与第j+1段轨迹在Tj时刻的k阶导数相等；k取1、2、3、4，表示第j段轨迹最后一个路径点与第j+1段轨迹第一个路径点的位置、速度、加速度以及加速度变化率分别对应相等；
由此，将路径优化问题转换为二次规划问题，在MATLAB中使用二次规划器，并调用quadprog函数，根据式(4)、(5)的约束条件，对式(3)的目标函数进行求解，完成初始轨迹的优化，得到一次优化轨迹。
第三步、对一次优化轨迹进行冗余路径点修剪，对轨迹重新添加约束条件，通过二次规划求解目标函数，得到二次优化轨迹；
如图2所示，一次优化轨迹尽管是连续、光滑的，由于A*算法的路径搜索严重依赖于栅格地图的分辨率，因此生成的轨迹并不是最短的，存在冗余路径点；为避免冗余路径点导致的轨迹长度问题，需要删除冗余路径点。
根据式(6)逐一判断除起始点、目标点以外的所有路径点是否为候选冗余路径点，若满足式(6)则为候选冗余路径节点，否则为正常路径点；
s-(l*1.05)＞0 (6)
式中，s＝s1+s2表示与待判断路径点相连的前后两段轨迹长度之和，s1表示待判断路径点与前一个路径点之间的轨迹长度，s2表示待判断路径点与后一个路径点之间的轨迹长度，l表示位于待判断路径点前、后两个路径点的欧式距离，系数1.05表示容许中间路径点偏离前后两个路径点连线的程度；
有些候选冗余路径点尽管满足式(6)，但是这些候选冗余路径点是为了绕开障碍物而必须存在的，应当保留，故逐一判断各个候选冗余路径点是否为冗余路径点，冗余路径点应当删除；如图4所示，对于任意候选冗余路径点，若该候选冗余路径点至其后一路径点的连线方向为左上方，则判断该候选冗余路径点正左侧或正上方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；若该候选冗余路径点至其后一路径点的连线方向为右上方，则判断该候选冗余路径点正右侧或正上方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；若该候选冗余路径点至其后一路径点的连线方向为左下方，则判断该候选冗余路径点正左侧或正下方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；若该候选冗余路径点至其后一路径点的连线方向为右下方，则判断该候选冗余路径点正右侧或正下方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；
删除冗余路径点后，与之对应的轨迹的约束也同时减少，为了防止无人飞行器与障碍物发生碰撞，需要为轨迹重新添加约束以进行二次规划；对于任意冗余路径点，连接该冗余路径点相邻的前、后两个路径点，生成飞行走廊的基线；以基线为基准向左、右两侧扩张飞行走廊，飞行走廊的左、右侧边界与邻近障碍物之间的间距d应满足下式：
Figure BDA0004091331990000051
式中，w表示无人飞行器的宽度；
飞行走廊的边界约束条件为：
fj(Tj)≤bj (8)
bj＝xobs+d (9)
式中，fj(Tj)表示第j段轨迹在Tj时刻的位置，bj表示第j段轨迹飞行走廊的边界位置，xobs表示与飞行走廊邻近的障碍物的横坐标；
在MATLAB中再次调用quadprog函数，根据式(5)、(8)的约束条件对式(3)的目标函数进行求解，得到二次优化轨迹；
至此，完成整个路径规划。
本发明未述及之处适用于现有技术。

Claims (1)

1.一种基于A*算法与改进最小化snap的无人飞行器路径规划方法，其特征在于，该方法包括以下步骤：
第一步、根据环境地图构建栅格地图，利用A*算法进行路径点搜索，得到初始轨迹；
第二步、利用最小化snap对初始轨迹进行优化，得到一次优化轨迹；
最小化snap优化的目标函数为：
Figure FDA0004091331980000011
式中，f(4)(t)表示第j段轨迹的加速度的二阶导数，Tj-1、Tj分别表示第j段轨迹的起始时刻和截止时刻，t表示时刻；
最小化snap优化的约束条件为：
Figure FDA0004091331980000012
Figure FDA0004091331980000013
式中，fj (k)(Tj)、
Figure FDA0004091331980000014
分别表示第j、j+1段轨迹在Tj时刻的k阶导数；式(4)表示第j段轨迹在Tj时刻的k阶导数需要满足的约束；式(5)表示第j段轨迹在Tj时刻的k阶导数与第j+1段轨迹在Tj时刻的k阶导数相等；k取1、2、3、4表示第j段轨迹最后一个路径点与第j+1段轨迹第一个路径点的位置、速度、加速度以及加速度变化率分别对应相等；
第三步、对一次优化轨迹进行冗余路径点修剪，对轨迹重新添加约束条件，通过二次规划求解目标函数，得到二次优化轨迹；
判断除起始点、目标点以外的各个路径点是否为候选冗余路径点，若满足式(6)则为候选冗余路径节点，否则为正常路径点；
s-(l*1.05)>0 (6)
式中，s＝s1+s2表示与待判断路径点相连的前后两段轨迹长度之和，s1表示待判断路径点与前一个路径点之间的轨迹长度，s2表示待判断路径点与后一个路径点之间的轨迹长度，l表示位于待判断路径点前、后两个路径点的欧式距离；
判断各个候选冗余路径点是否为冗余路径点，若为冗余路径点则删除；对于任意候选冗余路径点，若该候选冗余路径点至其后一路径点的连线方向为左上方，则判断该候选冗余路径点正左侧或正上方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；若该候选冗余路径点至其后一路径点的连线方向为右上方，则判断该候选冗余路径点正右侧或正上方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；若该候选冗余路径点至其后一路径点的连线方向为左下方，则判断该候选冗余路径点正左侧或正下方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；若该候选冗余路径点至其后一路径点的连线方向为右下方，则判断该候选冗余路径点正右侧或正下方紧邻的一个栅格内是否有障碍物，若是，则该候选冗余路径点应保留，否则该候选冗余路径点为冗余路径点，应删除；
对于任意冗余路径点，连接该冗余路径点相邻的前、后两个路径点，生成飞行走廊的基线；以基线为基准向左、右两侧扩张飞行走廊，飞行走廊的左、右侧边界与邻近障碍物之间的间距d应满足下式：
Figure FDA0004091331980000021
式中，w表示无人飞行器的宽度；
飞行走廊的边界约束条件为：
fj(Tj)≤bj (8)
bj＝xobs+d (9)
式中，fj(Tj)表示第j段轨迹在Tj时刻的位置，bj表示第j段轨迹飞行走廊的边界位置，xobs表示与飞行走廊邻近的障碍物的横坐标；
通过二次规划，根据式(5)、(8)的约束条件对式(3)的目标函数进行求解，得到二次优化轨迹，完成路径规划。
CN202310152583.9A 2023-02-23 2023-02-23 基于A*算法与改进最小化snap的无人飞行器路径规划方法 Pending CN116243724A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310152583.9A CN116243724A (zh) 2023-02-23 2023-02-23 基于A*算法与改进最小化snap的无人飞行器路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310152583.9A CN116243724A (zh) 2023-02-23 2023-02-23 基于A*算法与改进最小化snap的无人飞行器路径规划方法

Publications (1)

Publication Number Publication Date
CN116243724A true CN116243724A (zh) 2023-06-09

Family

ID=86632657

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310152583.9A Pending CN116243724A (zh) 2023-02-23 2023-02-23 基于A*算法与改进最小化snap的无人飞行器路径规划方法

Country Status (1)

Country Link
CN (1) CN116243724A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116449852A (zh) * 2023-06-13 2023-07-18 麦岩智能科技(北京)有限公司 一种轨迹规划方法、装置、电子设备及介质

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116449852A (zh) * 2023-06-13 2023-07-18 麦岩智能科技(北京)有限公司 一种轨迹规划方法、装置、电子设备及介质

Similar Documents

Publication Publication Date Title
CN110333714B (zh) 一种无人驾驶汽车路径规划方法和装置
CN107168305B (zh) 路口场景下基于Bezier和VFH的无人车轨迹规划方法
CN112378408A (zh) 一种实现轮式移动机器人实时避障的路径规划方法
CN109990787B (zh) 一种机器人在复杂场景中规避动态障碍物的方法
CN112683275B (zh) 一种栅格地图的路径规划方法
CN112731916A (zh) 融合跳点搜索法和动态窗口法的全局动态路径规划方法
CN112965485B (zh) 一种基于二次区域划分的机器人全覆盖路径规划方法
CN114281084B (zh) 一种基于改进a*算法的智能车全局路径规划方法
CN112577506A (zh) 一种自动驾驶局部路径规划方法和系统
CN116243724A (zh) 基于A*算法与改进最小化snap的无人飞行器路径规划方法
CN114527753A (zh) 人机共融的楼宇间路径规划方法、计算机装置及程序产品
CN116360457A (zh) 一种基于自适应栅格和改进a*-dwa融合算法的路径规划方法
CN115328208A (zh) 一种实现全局动态路径规划的无人机路径规划方法
CN114371716A (zh) 一种用于消防机器人的自动驾驶巡检方法
CN115167398A (zh) 一种基于改进a星算法的无人船路径规划方法
CN115268448A (zh) 一种基于冲突搜索和速度障碍的多机器人路径规划方法
CN116518974A (zh) 一种基于空域网格的无冲突航路规划方法
CN114967701A (zh) 一种动态环境下移动机器人自主导航方法
CN112665603B (zh) 一种基于带时间窗a*改进的多车路径规划方法
CN117249842A (zh) 一种基于轨迹平滑优化的无人车混合轨迹规划方法
CN114815899A (zh) 基于3d激光雷达传感器的无人机三维空间路径规划方法
Wang et al. Dynamic path planning algorithm for autonomous vehicles in cluttered environments
Li et al. DF-FS based path planning algorithm with sparse waypoints in unstructured terrain
Dang et al. A path planning method for indoor robots based on partial & global A-star algorithm
Neuman et al. Anytime policy planning in large dynamic environments with interactive uncertainty

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination