CN112148024A - 基于自适应伪谱法的无人机实时在线航迹规划方法 - Google Patents
基于自适应伪谱法的无人机实时在线航迹规划方法 Download PDFInfo
- Publication number
- CN112148024A CN112148024A CN202010843001.8A CN202010843001A CN112148024A CN 112148024 A CN112148024 A CN 112148024A CN 202010843001 A CN202010843001 A CN 202010843001A CN 112148024 A CN112148024 A CN 112148024A
- Authority
- CN
- China
- Prior art keywords
- aerial vehicle
- unmanned aerial
- time
- optimal control
- subinterval
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 92
- 238000005457 optimization Methods 0.000 claims abstract description 35
- 238000001228 spectrum Methods 0.000 claims abstract description 31
- 230000008569 process Effects 0.000 claims abstract description 23
- 230000008859 change Effects 0.000 claims abstract description 18
- 238000005070 sampling Methods 0.000 claims description 31
- 238000004364 calculation method Methods 0.000 claims description 20
- 238000004422 calculation algorithm Methods 0.000 claims description 19
- 239000011159 matrix material Substances 0.000 claims description 15
- 230000003044 adaptive effect Effects 0.000 claims description 8
- 238000005259 measurement Methods 0.000 claims description 6
- 238000011426 transformation method Methods 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 206010048669 Terminal state Diseases 0.000 claims description 3
- 150000001875 compounds Chemical class 0.000 claims description 3
- 239000000126 substance Substances 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 239000007787 solid Substances 0.000 claims description 2
- 230000009194 climbing Effects 0.000 description 3
- 230000001186 cumulative effect Effects 0.000 description 3
- 238000005315 distribution function Methods 0.000 description 3
- 230000007547 defect Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 239000000446 fuel Substances 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000007670 refining Methods 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000010187 selection method Methods 0.000 description 1
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/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提出一种基于自适应伪谱法的无人机实时在线航迹规划方法，属于单无人机航迹在线规划技术领域。该方法首先建立包含动力学方程、过程约束条件、机动性能约束条件、威胁区约束的无人机航迹优化的非线性最优控制模型；利用Radau伪谱法将连续时间的非线性最优控制模型转化为多区间的非线性最优控制模型，基于航迹的曲率密度函数实现伪谱法的自适应配点策略；通过连续在线重计算开环最优控制的方法实现闭环控制反馈，实现无人机在线航迹的实时优化。本发明能够在有限的时间内快速在线规划出一条可飞航迹，并且当感知外部环境改变时，能够根据新的信息重新调整新的航迹，收敛速度快，收敛精度高，在实际场景中有很强的的实用性。
Description
技术领域
本发明属于单无人机航迹规划技术领域，尤其是涉及一种基于自适应伪谱法的无人机实时在线航迹规划方法。
背景技术
在线航迹规划是针对不确定环境中的航迹规划问题，无人机需要在参考航迹的约束下，根据局部地形、地貌、障碍、威胁等信息以及自身机动能力的限制，实时的计算出一条飞行航迹，同时跟随该航迹完成飞行任务。即无人机的航迹规划算法向着任务场景的复杂化、障碍物的动态化等方面发展，而这些需求的不断提高，意味着无人机的在线航迹规划算法应该能够在有限的、尽可能短的时间内快速的规划出一段可飞航迹，且当感知到环境信息发生改变的，能够根据新的信息重新对路径进行寻优。
目前无人机的航迹规划算法可分为基于控制理论优化的规划算法(凸优化、滚动时域控制等)、基于启发信息的规划算法(A*算法、遗传算法、粒子群算法等)、基于神经网络的规划算法和基于人工势场的规划算法。但大多数的研究停留在离线航迹规划领域，且存在计算量大，鲁棒性不佳等缺陷。无人机的航迹优化问题可抽象为求解一组包含微分-代数约束和不等式约束的非线性最优控制问题，而这种实时、在线航迹生成的前提实际上是非线性控制系统的实时计算。
伪谱法也称为正交配点法属于求解非线性最优控制问题直接方法中的一种，相比其它直接解法，伪谱法在求解多约束条件下的非线性最优控制问题具有指数的收敛速度和求解精度，被认为具有实时求解非线性最优控制问题数值解的潜力。对于采用伪谱法求解非线性最优控制问题时，近似解的精度和计算效率与配点的选取方法和配置方式紧密相关，自适应的动态配点选取方式能够以更有效的代价得到更高精度的近似解。目前大多数文献研究的是基于伪谱法的无人机离线航迹优化问题，在单个无人机的航迹规划领域，目前尚无采用伪谱算法实现无人机的在线实时航迹规划任务的方法。
发明内容
本发明目的是为了克服已有技术的不足之处，提供一种基于自适应伪谱法的无人机实时在线航迹规划方法。本发明针对不确定的外部环境，利用自适应伪谱法和基于固定采样的实时反馈策略解决多约束条件下的单个无人机的实时在线航线规划问题。本发明针对单个无人机的航迹实时在线规划问题，建立了包含动力学方程、过程约束条件、机动性能约束条件、威胁区约束条件下的无人机航迹优化的非线性最优控制模型，同时考虑了阵风的干扰作用；利用自适应Radau伪谱法快速求解相应的非线性最优控制问题，为无人机实时规划出最优航迹；通过连续在线计算开环最优控制并作用于无人机系统的方式实现无人机在线航迹的实时优化。
本发明提出一种基于自适应伪谱法的无人机实时在线航迹规划方法，其特征在于，该方法包括以下步骤：
1)建立无人机航迹规划的非线性最优控制模型，具体步骤如下：
1.1)建立无人机航迹规划的动力学方程：
式中，i表示时刻序号；xi,yi,zi分别表示i时刻无人机在x,y,z三个方向上的位置；V表示无人机的飞行速度；γi,ψi分别示i时刻无人机的航迹倾角和航向角；ui1,ui2分别表示i时刻航迹倾角的变化率和航向角的变化率，ui1,ui2为i时刻作用于无人机上的控制量；Wix,Wiy分别表示i时刻无人机受到的阵风在x,y两个方向上的扰动；则i时刻无人机状态空间矩阵Xi和控制空间矩阵Ui分别定义为：
Xi＝[xi yi zi Vγi ψi]T，Ui＝[ui1 ui2]T
1.2)确定无人机航迹规划的非线性最优控制模型约束条件，具体如下：
1.2.1)过程约束；
1.2.1.1)最大爬升角度和最大偏航角度约束；
其中，γmax表示无人机的最大爬升角度；ψmax表示无人机的最大航向角度；(xi,yi,zi),(xi-1,yi-1,zi-1)分别表示i时刻和i-1时刻无人机的位置坐标，且ai＝(xi-xi-1,yi-yi-1)，ai表示i时刻与i-1时刻无人机在水平x方向和y方向上的位置差；
1.2.1.2)位置、速度、地形约束；
其中，la表示第a航段无人机的飞行距离，lmax表示最大航迹距离；
zmax≥za≥zmin (5)
其中，zmin,zmax分别表示整个飞行过程中无人机的最小高度和最大高度；za表示第a航段无人机的飞行高度；
1.2.2)机动性能约束；
1.2.2.1)航向角和航迹倾角约束；
γmin≤γi≤γmax,ψmin≤ψi≤ψmax (6)
其中，γmin,γmax分别表示无人机的最小航迹倾角和最大航迹倾角；ψmin,ψmax分别表示无人机的最小航向角和最大航向角；
|ui1|≤u1max,|ui2|≤u2max (7)
其中，u1max,u2max分别表示无人机的最大航迹倾角变化率和最大航向角变化率；u2max对应于无人机i时刻的的最小转弯半径rimin，满足：
1.2.3)威胁区约束；
将雷达所能触及的区域视为无人机飞行过程中的威胁区，设(xcj,ycj)表示第j个威胁区的坐标中心，Rj表示第j个威胁区的半径，则每个威胁区对应的威胁区约束表达式如下：
其中，S(j)表示第j个威胁区对应的威胁约束，下标j表示威胁区的序号，J表示威胁区的总个数；Δxj＝xi-xcj,Δyj＝yi-ycj，分别表示i时刻无人机与第j个威胁区威胁中心在水平x方向和y方向上的距离；
1.3)建立无人机航迹规划的非线性最优控制模型，目标函数表达式如下：
满足约束如下：
其中，t0表示无人机起飞的初始时刻，tf表示无人机到达目标点的终端时刻；
式(11)中第一个式子
代表式(1)的动态约束；第二个式子代表步骤1.2)中的机动性能约束，第三个式子代表步骤1.2)中的过程约束；
2)利用步骤1)建立的模型，对步骤1.2)中的威胁区约束进行转化，建立无人机航迹规划的多区间非线性最优控制模型；具体步骤如下：
2.1)将每个威胁区看作整个航迹的一个内点，将步骤1.2.3)中的威胁区约束转化为对应的内点约束：
其中，S(1)表示S关于时间t的一阶导数；
2.2)通过步骤2.1)得到的内点约束将步骤1)中的模型转化为多区间最优控制模型；具体转化方法如下：
2.2.1)将无人机飞行的原始时间区间I＝[t0,tf]划分为J+1个子区间，其中第p个子区间为Ip＝[tp-1，tp]，p＝1,2,…,J+1，J表示威胁区的总数，满足
Φ表示空集，p表示子区间序号；当p＝J+1时，tp＝tf；
2.2.2)每个相邻子区间的状态连续，满足下式：
其中，
分别表示无人机完成第p个子区间飞行时的终端状态和终端时刻；
分别表示无人机进入第p+1个子区间飞行时的初始状态和初始时刻；
则步骤1)建立的无人机航迹规划的非线性最优控制模型转化为多区间非线性最优控制模型，表达式如下：
式中，J(P)表示如式(10)所示的最优控制模型在第p个子区间的目标函数；
式(14)的目标函数满足约束条件：
其中，x(p)(t)，u(p)(t)分别表示p∈[1,…,J+1]子区间内的状态变量、控制变变量；
C(p)分别表示p子区间内的无人机受到的动态约束、机动性能约束和过程不等式约束；P(s)表示子区间之间的内点约束，
表示与第s连接点相邻的左侧子区间序号，s＝1,2…,J，
为与第s连接点相邻的右侧子区间序号；
2.2.3)将每个子区间[tp-1,tp]对应的时间t(p)转化为区间[-1,1]内的时间τ(p)，转化方法如下：
3)利用基于密度函数的自适应Radau伪谱法在线连续求解开环最优控制，结合固
定采样频率的方式将得到的开环最优控制量在固定的周期内作用于无人机上，实现无人机系统闭环控制的反馈，完成无人机的实时在线航迹规划；具体步骤如下：
3.1)设定无人机系统的采样周期ΔT，设定无人机在t0时刻的初始状态x0＝x(t0)；
3.2)利用基于密度函数的自适应Radau伪谱优化算法对步骤2)得到的多区间非线性最优控制模型进行离线优化求解，计算得到当前t0时刻优化对应的开环最优控制量
具体步骤如下：
3.2.1)令Nk＝M表示第k个子区间[tk-1,tk]内的配点数，对该子区间取每个相邻配点的中点，
作为该子区间的采样点，i＝1,..,M-1；
3.2.2)将动态约束方程
在步骤3.2.1)得到采样点上的残差作为求解精度的判定准则：
式中，
表示状态变量在第k个子区间的采样点处的近似值，是一个(Nk-1)×n维的矩阵，其中，n表示动力学方程式(1)中状态变量的个数；
3.2.3)判定：若
中的每个元素都比ε小时，则认为该子区间满足求解精度要求，其中，ε为设定的误差限；
当所有的子区间都满足求解精度要求时，则判定求解得到符合要求的最优控制量，然后进入步骤3.3)；
若
中存在任一元素不小于ε时，则认为该子区间不满足求解精度要求，进入步骤3.2.4)，对该子区间的求解精度进行改进；
3.2.4)将不满足求解精度的第k个子区间拆分为nk个子区间；
其中，新增的子区间点的位置由式
确定，i为正整数；
3.2.5)对于经过步骤3.2.4)拆分后的每个子区间分别设置M个配点，然后重新返回步骤3.2.1)；
3.3)在第一个采样周期[t0,t0+ΔT]内，将利用步骤3.2)得到的最优控制量
作用于无人机的控制系统上，并预报t0+ΔT时刻无人机的状态值
记预报时间为tp1；
以
为初始条件，在[t0+ΔT,tf]内，利用步骤3-2)的自适应Radau伪谱法在线优化计算新的最优控制量
记优化计算时间为tg1；
表示t0+ΔT时刻无人机的预报状态；
3.4)以无人机的状态实际测量值xi＝x(t0+iΔT)为初始条件，i＝1,2…，将步骤3.3)在线计算得到的最优控制量
在新的采样周期[t0+iΔT,t0+(i+1)ΔT]内作用于无人机的控制系统上，预报ti+1时刻的无人机的状态值
记预报计算时间为tpi；
3.5)以新的状态预报值
为新初始条件，在[t0+(i+1)ΔT,tf]内，利用步骤3-2)的自适应Radau伪谱法实时在线优化计算新的最优控制量
记优化计算时间为tgi；
3.6)搜集无人机的状态实际测量值x(t0+(i+1)ΔT)并进行判定：
若tp(i+1)+tg(i+1)≤ΔT，则令i＝i+1，返回步骤3.4)；
若tp(i+1)+tg(i+1)＞ΔT，则在
内，继续应用前一周期优化得到的开环最优控制
同时预报计算t0+(i+2)ΔT时刻状态
然后重新返回步骤3.4)，实现对无人机的在线航迹实时规划。
本发明提出的一种基于自适应伪谱法的无人机实时在线航迹规划方法，针对不确定的航迹规划环境，基于伪谱离散化的航迹优化算法和连续在线重计算开环最优控制的实时航迹规划方法，解决多约束条件下的单无人机在线航迹的实时优化问题。相比现有技术，具有以下有益效果：
1)相对于单无人机的离线航迹规划方法，本发明能够在有限的、尽可能短的时间内快速在线规划出一条可飞航迹，并且当感知外部环境改变时，能够根据新的信息重新调整新的航迹。
2)本发明解决了单无人机在线航迹的实时规划问题，为提高算法执行效率，在采用Radau伪谱法求解非线性规划问题的同时，结合航迹曲率密度函数及其累积函数的性质实现自适应伪谱航迹规划算法，用于快速求解相应的航迹规划问题，本发明方法具有收敛速度快，收敛精度高的优点。
3)和离线轨迹优化算法不同，本发明通过固定采样频率下的连续、在线生成开环最优控制的方式提供无人机系统的闭环反馈，实现无人机航迹的实时、在线航迹规划，提高该方法在实际场景中的实用性和高效性。
附图说明
图1是本发明提出的基于自适应伪谱法的无人机实时在线航迹规划方法的整体流程图。
图2是本发明方法中固定采样频率下，开环最优控制的连续应用流程图。
图3是本发明方法中的自适应Radau伪谱优化算法流程图。
图4为发明实施例中累积分布函数的示意图。
具体实施方式
本发明提出一种基于自适应伪谱法的无人机实时在线航迹规划方法，下面结合附图和具体实施过程对本发明作进一步详细说明。
本发明提出一种基于自适应伪谱法的无人机实时在线航迹规划方法，特别适用于单个无人机在任务场景复杂化、障碍物动态化等多约束条件下的实时航迹规划，该方法整体流程如图1所示，本方法具体包括以下步骤：
1)建立无人机航迹规划的非线性最优控制模型，具体步骤如下：
1.1)考虑随机风场对航迹的影响，建立无人机航迹规划的动力学方程如下：
式中，i表示时刻序号；xi,yi,zi分别表示i时刻无人机在x,y,z三个方向上的位置；V表示无人机的飞行速度(定值)；γi,ψi分别示i时刻无人机的航迹倾角和航向角；ui1,ui2分别表示i时刻航迹倾角的变化率和航向角的变化率，即i时刻作用于无人机上的控制量；Wix,Wiy分别表示i时刻无人机受到的阵风在x,y两个方向上的扰动。
则i时刻状态空间矩阵Xi和控制空间矩阵Ui分别定义为：
Xi＝[xi yi zi Vγi ψi]T，Ui＝[ui1 ui2]T。
1.2)确定无人机航迹规划的非线性最优控制模型约束条件，具体如下：
1.2.1)过程约束；
1.2.1.1)最大爬升角度和最大偏航角度约束；
无人机在纵向平面的抬头或低头，爬升角度会影响无人机的安全性能，故爬升角度不可过大，否则会导致无人机失速，一般飞行状态下，最大爬升角近似等于俯仰角，即：
同时，无人机在水平面内，由于受到自身性能的约束，只能在一定的范围内转弯，即：
其中，γmax表示无人机的最大爬升角度；ψmax表示无人机的最大航向角度；(xi,yi,zi),(xi-1,yi-1,zi-1)分别表示i时刻和i-1时刻无人机的位置坐标，且ai＝(xi-xi-1,yi-yi-1)，ai表示i时刻与i-1时刻无人机在水平x方向和y方向上的位置差。
1.2.1.2)位置、速度、地形约束；
受自身性能的影响(燃油、可飞时间等)，无人机所有航段的飞行距离之和应小于最大航迹距离，即：
其中，la表示第a航段无人机的飞行距离，lmax表示最大航迹距离。
此外，飞行过程中还要考虑地形因素对无人机造成的威胁，对整个飞行高度施加如下约束：
zmax≥za≥zmin (5)
其中，zmin,zmax分别表示整个飞行过程中无人机的最小高度和最大高度；za表示第a航段无人机的飞行高度；
1.2.2)机动性能约束；
1.2.2.1)航向角和航迹倾角约束；
机动性能反映了无人机改变速度、高度、速度方向的能力。由无人机自身性能的限制，
飞行过程中的航迹倾角和航向角不可过大，约束如下：
γmin≤γi≤γmax,ψmin≤ψi≤ψmax (6)
其中，γmin,γmax分别表示无人机的最小和最大航迹倾角；ψmin,ψmax分别表示无人机的最小航向角和最大航向角。
为便于限制执行机构的变化速率，对航迹倾角的变化率和航向角的变化率的约束分别为：
|ui1|≤u1max,|ui2|≤u2max (7)
其中，u1max,u2max分别表示无人机的最大航迹倾角变化率和最大航向角变化率；u2max对应于无人机i时刻的的最小转弯半径rimin，满足：
1.2.3)威胁区约束；
将雷达所能触及的区域视为无人机飞行过程中的威胁区，将每个威胁区看作一个具有无穷高度的圆形区域(即圆柱体)，设(xcj,ycj)表示第j个威胁区的坐标中心，Rj表示第j个威胁区的半径，则每个威胁区对应的威胁区约束可统一用下式表示：
等式左边符号S(j)表示第j个威胁区对应的威胁约束，相当于给了右端式子的一个简化表示代号，整个式子表示无人机在t时刻的位置与威胁区域中心之间的距离应大于威胁区域的半径，否则无人机便进入雷达威胁区域内了。
其中，下标j表示威胁区的序号，J表示威胁区的总个数；Δxj＝x(t)-xcj,Δyj＝y(t)-ycj，分别表示t时刻无人机与第j个威胁区威胁中心在水平x方向和y方向上的距离，
其中，下标j表示威胁区的序号，J表示威胁区的总个数；Δxj＝xi-xcj,Δyj＝yi-ycj，分别表示i时刻无人机与第j个威胁区威胁中心在水平x方向和y方向上的距离；
1.3)确定无人机航迹规划的非线性最优控制模型的目标函数；
考虑从起点到终点，以飞行时间最短作为优化的性能指标，则所述的无人机航迹规划的非线性最优控制模型的目标函数可描述为：
满足约束如下：
其中，t0表示无人机起飞的初始时刻，tf表示无人机到达目标点的终端时刻(t0到tf之间可以是有限的，也可以趋向于无穷)。
式(11)中第一个式子
代表式(1)所表示的动态约束；第二个式子代表步骤1.2)中的机动性能约束，第三个式子代表步骤1.2)中的过程约束。
步骤1)建立的模型为一个单区间最优控制模型。
2)利用步骤1)建立的模型，对步骤1.2)中的威胁区约束进行转化，建立无人机航迹规划的多区间非线性最优控制模型；具体步骤如下：
2.1)将每个威胁区看作整个航迹的一个内点，将步骤1.2.3)中的威胁区约束转化为对应的内点约束：
其中，S(1)表示S关于时间t的一阶导数。
2.2)通过步骤2.1)中得到的内点约束，将步骤1)中的单区间最优控制模型转化为多区间最优控制模型，并统一于一个优化框架内；具体转化过程如下：
2.2.1)将无人机飞行的原始时间区间I＝[t0,tf]划分为J+1个子区间，其中第p个子区间Ip＝[tp-1，tp]，(p＝1，2，…，J+1)，(当p＝J+1时，tp＝tf)J表示威胁区的总数，满足
Φ表示空集，p表示子区间序号。
2.2.2)为了保证各个子区间的连续性，要求相邻子区间的状态连续，即：
其中，
分别表示无人机完成第p个子区间飞行时的终端状态和终端时刻；
分别表示无人机进入第p+1个子区间飞行时的初始状态和初始时刻。
则步骤1)建立的无人机航迹规划的非线性最优控制模型转化为多区间非线性最优控制模型描述为最小化代价函数，表达式如下：
式中，J(P)表示如式(10)所示的最优控制模型在第p个子区间的目标函数。
式(14)的目标函数满足约束条件：
其中，x(p)(t)，u(p)(t)分别表示p∈[1,…,J+1]子区间内的状态变量、控制变变量；
C(p)分别表示p子区间内的无人机受到的动态约束、机动性能约束和过程不等式约束；P(s)表示子区间之间的内点约束，
表示与第s连接点相邻的左侧子区间序号，
为与第s连接点相邻的右侧子区间序号。
2.2.3)将每个子区间[tp-1,tp]对应的时间t(p)转化为区间[-1,1]内的时间τ(p)，转化方法如下：
在每个子区间[tp-1,tp]内，以Radau伪谱法作为基本的离散化方法，将相应的最优控制模型转变为非线性规划模型进行求解。
3)利用基于密度函数的自适应Radau伪谱法在线、连续求解开环最优控制，结合固
定采样频率的方式将得到的开环最优控制量在固定的周期内作用于无人机上，实现无人机系统闭环控制的反馈，完成无人机的实时在线航迹规划，其流程图参见图2。具体步骤如下：
3.1)设定无人机系统的采样周期ΔT，设定无人机在t0时刻的初始状态x0＝x(t0)(该初始状态具体会根据实际问题提前设定)；
3.2)利用基于密度函数的自适应Radau伪谱优化算法对步骤2)得到的多区间非线性最优控制模型进行离线优化求解，计算得到当前t0时刻优化对应的开环最优控制量
(其中，每一次优化都是从该次优化时的当前时刻到终端时刻的一个整体优化过程)。基于密度函数的自适应Radau伪谱优化算法，流程图参见图3所示，具体实施过程如下：
3.2.1)令Nk＝M表示第k个子区间[tk-1,tk]内的配点数(M为选定的数值，每个子区间的初始设定数值是相同的，可根据具体情况设定)，对该子区间取每个相邻配点的中点，
作为该子区间的采样点；
3.2.2)将动态约束方程
在步骤3.2.1)得到采样点上的残差作为求解精度的判定准则：
式中，
表示状态变量在第k个子区间的采样点处的近似值，是一个(Nk-1)×n维的矩阵，其中，n表示动力学方程式(1)中状态变量的个数(本发明中n＝6)；
3.2.3)判定：若
中的每个元素都比ε小时，则认为该子区间满足求解精度要求，其中，ε为设定的误差限；
当所有的子区间都满足求解精度要求时，则认为求解得到符合要求的最优控制量，然后进入步骤3.3)；
若
中存在任一元素不小于ε时，则认为该子区间不满足求解精度要求，进入步骤3.2.4)，采用以下策略对该子区间的求解精度进行改进。
3.2.4)若该子区间不满足求解精度要求，采用进一步细化该子区间的方式以提高求解精度，定义将不满足求解精度的第k个子区间拆分为nk个子区间(nk为设定的具体数值，其设定方式要同时兼顾求解非线性最优控制模型时的求解精度和计算代价，一般可设为2或3)。
其中，新增的子区间点的位置由式
确定，i为正整数，具体方法如下：
3.2.4.1)设x(k)表示第k个子区间[tk-1,tk]内优化得到的离散状态轨迹，相应的轨迹曲率函数定义为
该轨迹曲率函数对应的曲率密度函数为ρ(τ)＝cκ(τ)1/3，
其中，c为一个选定的常数，满足
与该曲率密度函数对应的累积分布函数为
根据密度函数及其累积分布函数的性质和图像(参见图4)可知，当在区间[a,b]内插入N个点
时，若已知第i个点ti的位置，那么第i+1个点ti+1的位置可由式
确定。)
3.2.5)对于经过步骤3.2.4)拆分后的每个子区间分别设置M个配点，然后重新返回步骤3.2.1)
3.3)在第一个采样周期[t0,t0+ΔT]内，将利用步骤3.2)离线计算得到的最优控制量
作用于无人机的控制系统上，并预报t0+ΔT时刻无人机的状态值
记预报计算时间为tp1；(该时间为计算机计算
所需要的时间，实际上非常非常短)
以
为初始条件，在[t0+ΔT,tf]内，利用步骤3-2)的自适应Radau伪谱法在线优化计算新的最优控制量
记优化计算时间为tg1；
表示t0+ΔT时刻无人机的预报状态；
3.4)以无人机的状态实际测量值xi＝x(t0+iΔT)为初始条件，i＝1,2…，(xi为向量形式)，将步骤3.3)在线计算得到的最优控制量
在新的采样周期[t0+iΔT,t0+(i+1)ΔT]内作用于无人机的控制系统上，预报ti+1时刻的无人机的状态值
记预报计算时间为tpi；
3.5)以新的状态预报值
为新初始条件，在[t0+(i+1)ΔT,tf]内，利用步骤3-2)的自适应Radau伪谱法实时在线优化计算新的最优控制量
记优化计算时间为tgi；
3.6)搜集无人机的状态实际测量值x(t0+(i+1)ΔT)并进行判定：
若tp(i+1)+tg(i+1)≤ΔT，则令i＝i+1，返回步骤3.4)；若tp(i+1)+tg(i+1)＞ΔT，则在[t0+(i+1)ΔT,t0+(i+2)ΔT]内，继续应用前一周期优化得到的开环最优控制
同时预报计算t0+(i+2)ΔT时刻状态
然后重新返回步骤3.4)，实现对无人机的在线航迹实时规划。
无人机采用上述步骤通过连续、在线计算开环最优控制并于固定的采样周期连续作用于控制系统的方式实现闭环反馈，能够实现无人机的在线航迹实时规划，并且额能够实现当感知环境信息发生变化时，根据新的信息重新对航迹进行寻优。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。
Claims (1)
1.一种基于自适应伪谱法的无人机实时在线航迹规划方法，其特征在于，该方法包括以下步骤：
1)建立无人机航迹规划的非线性最优控制模型，具体步骤如下：
1.1)建立无人机航迹规划的动力学方程：
式中，i表示时刻序号；xi,yi,zi分别表示i时刻无人机在x,y,z三个方向上的位置；V表示无人机的飞行速度；γi,ψi分别示i时刻无人机的航迹倾角和航向角；ui1,ui2分别表示i时刻航迹倾角的变化率和航向角的变化率，ui1,ui2为i时刻作用于无人机上的控制量；Wix,Wiy分别表示i时刻无人机受到的阵风在x,y两个方向上的扰动；则i时刻无人机状态空间矩阵Xi和控制空间矩阵Ui分别定义为：
Xi＝[xi yi zi Vγi ψi]T，Ui＝[ui1 ui2]T
1.2)确定无人机航迹规划的非线性最优控制模型约束条件，具体如下：
1.2.1)过程约束；
1.2.1.1)最大爬升角度和最大偏航角度约束；
其中，γmax表示无人机的最大爬升角度；ψmax表示无人机的最大航向角度；(xi,yi,zi),(xi-1,yi-1,zi-1)分别表示i时刻和i-1时刻无人机的位置坐标，且ai＝(xi-xi-1,yi-yi-1)，ai表示i时刻与i-1时刻无人机在水平x方向和y方向上的位置差；
1.2.1.2)位置、速度、地形约束；
其中，la表示第a航段无人机的飞行距离，lmax表示最大航迹距离；
zmax≥za≥zmin (5)
其中，zmin,zmax分别表示整个飞行过程中无人机的最小高度和最大高度；za表示第a航段无人机的飞行高度；
1.2.2)机动性能约束；
1.2.2.1)航向角和航迹倾角约束；
γmin≤γi≤γmax,ψmin≤ψi≤ψmax (6)
其中，γmin,γmax分别表示无人机的最小航迹倾角和最大航迹倾角；ψmin,ψmax分别表示无人机的最小航向角和最大航向角；
|ui1|≤u1max,|ui2|≤u2max (7)
其中，u1max,u2max分别表示无人机的最大航迹倾角变化率和最大航向角变化率；u2max对应于无人机i时刻的的最小转弯半径rimin，满足：
1.2.3)威胁区约束；
将雷达所能触及的区域视为无人机飞行过程中的威胁区，设(xcj,ycj)表示第j个威胁区的坐标中心，Rj表示第j个威胁区的半径，则每个威胁区对应的威胁区约束表达式如下：
其中，S(j)表示第j个威胁区对应的威胁约束，下标j表示威胁区的序号，J表示威胁区的总个数；Δxj＝xi-xcj,Δyj＝yi-ycj，分别表示i时刻无人机与第j个威胁区威胁中心在水平x方向和y方向上的距离；
1.3)建立无人机航迹规划的非线性最优控制模型，目标函数表达式如下：
满足约束如下：
其中，t0表示无人机起飞的初始时刻，tf表示无人机到达目标点的终端时刻；
式(11)中第一个式子
代表式(1)的动态约束；第二个式子代表步骤1.2)中的机动性能约束，第三个式子代表步骤1.2)中的过程约束；
2)利用步骤1)建立的模型，对步骤1.2)中的威胁区约束进行转化，建立无人机航迹规划的多区间非线性最优控制模型；具体步骤如下：
2.1)将每个威胁区看作整个航迹的一个内点，将步骤1.2.3)中的威胁区约束转化为对应的内点约束：
其中，S(1)表示S关于时间t的一阶导数；
2.2)通过步骤2.1)得到的内点约束将步骤1)中的模型转化为多区间最优控制模型；具体转化方法如下：
2.2.1)将无人机飞行的原始时间区间I＝[t0,tf]划分为J+1个子区间，其中第p个子区间为Ip＝[tp-1,tp]，p＝1,2,…,J+1，J表示威胁区的总数，满足
Φ表示空集，p表示子区间序号；当p＝J+1时，tp＝tf；
2.2.2)每个相邻子区间的状态连续，满足下式：
其中，
分别表示无人机完成第p个子区间飞行时的终端状态和终端时刻；
分别表示无人机进入第p+1个子区间飞行时的初始状态和初始时刻；
则步骤1)建立的无人机航迹规划的非线性最优控制模型转化为多区间非线性最优控制模型，表达式如下：
式中，J(P)表示如式(10)所示的最优控制模型在第p个子区间的目标函数；
式(14)的目标函数满足约束条件：
其中，x(p)(t)，u(p)(t)分别表示p∈[1,…,J+1]子区间内的状态变量、控制变变量；
C(p)分别表示p子区间内的无人机受到的动态约束、机动性能约束和过程不等式约束；P(s)表示子区间之间的内点约束，
表示与第s连接点相邻的左侧子区间序号，s＝1,2…,J，
为与第s连接点相邻的右侧子区间序号；
2.2.3)将每个子区间[tp-1,tp]对应的时间t(p)转化为区间[-1,1]内的时间τ(p)，转化方法如下：
3)利用基于密度函数的自适应Radau伪谱法在线连续求解开环最优控制，结合固
定采样频率的方式将得到的开环最优控制量在固定的周期内作用于无人机上，实现无人机系统闭环控制的反馈，完成无人机的实时在线航迹规划；具体步骤如下：
3.1)设定无人机系统的采样周期ΔT，设定无人机在t0时刻的初始状态x0＝x(t0)；
3.2)利用基于密度函数的自适应Radau伪谱优化算法对步骤2)得到的多区间非线性最优控制模型进行离线优化求解，计算得到当前t0时刻优化对应的开环最优控制量
具体步骤如下：
3.2.1)令Nk＝M表示第k个子区间[tk-1,tk]内的配点数，对该子区间取每个相邻配点的中点，
作为该子区间的采样点，i＝1,..,M-1；
3.2.2)将动态约束方程
在步骤3.2.1)得到采样点上的残差作为求解精度的判定准则：
式中，
表示状态变量在第k个子区间的采样点处的近似值，是一个(Nk-1)×n维的矩阵，其中，n表示动力学方程式(1)中状态变量的个数；
3.2.3)判定：若
中的每个元素都比ε小时，则认为该子区间满足求解精度要求，其中，ε为设定的误差限；
当所有的子区间都满足求解精度要求时，则判定求解得到符合要求的最优控制量，然后进入步骤3.3)；
若
中存在任一元素不小于ε时，则认为该子区间不满足求解精度要求，进入步骤3.2.4)，对该子区间的求解精度进行改进；
3.2.4)将不满足求解精度的第k个子区间拆分为nk个子区间；
其中，新增的子区间点的位置由式
确定，i为正整数；
3.2.5)对于经过步骤3.2.4)拆分后的每个子区间分别设置M个配点，然后重新返回步骤3.2.1)；
3.3)在第一个采样周期[t0,t0+ΔT]内，将利用步骤3.2)得到的最优控制量
作用于无人机的控制系统上，并预报t0+ΔT时刻无人机的状态值
记预报时间为tp1；
以
为初始条件，在[t0+ΔT,tf]内，利用步骤3-2)的自适应Radau伪谱法在线优化计算新的最优控制量
记优化计算时间为tg1；
表示t0+ΔT时刻无人机的预报状态；
3.4)以无人机的状态实际测量值xi＝x(t0+iΔT)为初始条件，i＝1,2…，将步骤3.3)在线计算得到的最优控制量
在新的采样周期[t0+iΔT,t0+(i+1)ΔT]内作用于无人机的控制系统上，预报ti+1时刻的无人机的状态值
记预报计算时间为tpi；
3.5)以新的状态预报值
为新初始条件，在[t0+(i+1)ΔT,tf]内，利用步骤3-2)的自适应Radau伪谱法实时在线优化计算新的最优控制量
记优化计算时间为tgi；
3.6)搜集无人机的状态实际测量值x(t0+(i+1)ΔT)并进行判定：
若tp(i+1)+tg(i+1)≤ΔT，则令i＝i+1，返回步骤3.4)；
若tp(i+1)+tg(i+1)＞ΔT，则在[t0+(i+1)ΔT,t0+(i+2)ΔT]内，继续应用前一周期优化得到的开环最优控制
同时预报计算t0+(i+2)ΔT时刻状态
然后重新返回步骤3.4)，实现对无人机的在线航迹实时规划。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010843001.8A CN112148024B (zh) | 2020-08-20 | 2020-08-20 | 基于自适应伪谱法的无人机实时在线航迹规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010843001.8A CN112148024B (zh) | 2020-08-20 | 2020-08-20 | 基于自适应伪谱法的无人机实时在线航迹规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112148024A true CN112148024A (zh) | 2020-12-29 |
| CN112148024B CN112148024B (zh) | 2022-04-08 |
Family
ID=73888534
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010843001.8A Active CN112148024B (zh) | 2020-08-20 | 2020-08-20 | 基于自适应伪谱法的无人机实时在线航迹规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112148024B (zh) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112923904A (zh) * | 2021-01-26 | 2021-06-08 | 四川融鑫信息科技有限公司 | 面向多无人机协同摄影测量的地质灾害隐患探测方法 |
| CN113008222A (zh) * | 2021-02-20 | 2021-06-22 | 西北工业大学 | 一种基于连续时间轨迹函数的航迹约束目标跟踪方法 |
| CN113050687A (zh) * | 2021-03-19 | 2021-06-29 | 四川大学 | 一种多无人机编队重组轨迹规划方法 |
| CN113157001A (zh) * | 2021-05-25 | 2021-07-23 | 北京航空航天大学 | 一种基于二阶锥优化的无人机路径规划方法 |
| CN113985895A (zh) * | 2021-11-29 | 2022-01-28 | 佛山市毕佳索智能科技有限公司 | 一种基于最优化的agv路径跟踪方法 |
| CN114089780A (zh) * | 2022-01-21 | 2022-02-25 | 中国电子科技集团公司第二十八研究所 | 一种面向城市空间的多旋翼无人机路径规划方法 |
| CN114117633A (zh) * | 2021-11-18 | 2022-03-01 | 中国人民解放军国防科技大学 | 一种无人机信息收集的控制方法及系统 |
| CN114265425A (zh) * | 2021-11-30 | 2022-04-01 | 中国电子科技集团公司第五十四研究所 | 一种多旋翼无人机编队防撞控制方法 |
| CN117970952A (zh) * | 2024-03-28 | 2024-05-03 | 中国人民解放军海军航空大学 | 无人机机动策略离线建模方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102269593A (zh) * | 2010-06-01 | 2011-12-07 | 北京航空航天大学 | 基于模糊虚拟力的无人机航路规划方法 |
| CN104317305A (zh) * | 2014-10-23 | 2015-01-28 | 中国运载火箭技术研究院 | 一种面向复杂战场威胁的射前航迹确定方法 |
| CN104536454A (zh) * | 2014-12-05 | 2015-04-22 | 中国运载火箭技术研究院 | 一种用于双无人机协同的时空同步匹配方法 |
| CN108388270A (zh) * | 2018-03-21 | 2018-08-10 | 天津大学 | 面向安全域的集群无人机轨迹姿态协同控制方法 |
| CN109858106A (zh) * | 2019-01-11 | 2019-06-07 | 南京航空航天大学 | 基于高斯伪谱法的飞行器小翼伸缩量优化方法 |
-
2020
- 2020-08-20 CN CN202010843001.8A patent/CN112148024B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102269593A (zh) * | 2010-06-01 | 2011-12-07 | 北京航空航天大学 | 基于模糊虚拟力的无人机航路规划方法 |
| CN104317305A (zh) * | 2014-10-23 | 2015-01-28 | 中国运载火箭技术研究院 | 一种面向复杂战场威胁的射前航迹确定方法 |
| CN104536454A (zh) * | 2014-12-05 | 2015-04-22 | 中国运载火箭技术研究院 | 一种用于双无人机协同的时空同步匹配方法 |
| CN108388270A (zh) * | 2018-03-21 | 2018-08-10 | 天津大学 | 面向安全域的集群无人机轨迹姿态协同控制方法 |
| CN109858106A (zh) * | 2019-01-11 | 2019-06-07 | 南京航空航天大学 | 基于高斯伪谱法的飞行器小翼伸缩量优化方法 |
Non-Patent Citations (3)
| Title |
|---|
| ZHENZHONG CHU等: "Underwater Vehicle Trajectory Planning in Dynamic Environments Based on Radau Pseudospectral Method", 《IEEE》 * |
| 王杰 等: "基于Radau伪谱法的SAR末制导成像段弹道优化", 《现代防御技术》 * |
| 邵士凯 等: "基于改进高斯伪谱法的多无人机协同轨迹规划", 《河北科技大学学报》 * |
Cited By (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112923904A (zh) * | 2021-01-26 | 2021-06-08 | 四川融鑫信息科技有限公司 | 面向多无人机协同摄影测量的地质灾害隐患探测方法 |
| CN112923904B (zh) * | 2021-01-26 | 2022-09-23 | 四川融鑫信息科技有限公司 | 面向多无人机协同摄影测量的地质灾害隐患探测方法 |
| CN113008222A (zh) * | 2021-02-20 | 2021-06-22 | 西北工业大学 | 一种基于连续时间轨迹函数的航迹约束目标跟踪方法 |
| CN113008222B (zh) * | 2021-02-20 | 2023-03-31 | 西北工业大学 | 一种基于连续时间轨迹函数的航迹约束目标跟踪方法 |
| CN113050687A (zh) * | 2021-03-19 | 2021-06-29 | 四川大学 | 一种多无人机编队重组轨迹规划方法 |
| CN113157001B (zh) * | 2021-05-25 | 2022-06-03 | 北京航空航天大学 | 一种基于二阶锥优化的无人机路径规划方法 |
| CN113157001A (zh) * | 2021-05-25 | 2021-07-23 | 北京航空航天大学 | 一种基于二阶锥优化的无人机路径规划方法 |
| CN114117633A (zh) * | 2021-11-18 | 2022-03-01 | 中国人民解放军国防科技大学 | 一种无人机信息收集的控制方法及系统 |
| CN113985895A (zh) * | 2021-11-29 | 2022-01-28 | 佛山市毕佳索智能科技有限公司 | 一种基于最优化的agv路径跟踪方法 |
| CN113985895B (zh) * | 2021-11-29 | 2024-05-03 | 佛山市毕佳索智能科技有限公司 | 一种基于最优化的agv路径跟踪方法 |
| CN114265425A (zh) * | 2021-11-30 | 2022-04-01 | 中国电子科技集团公司第五十四研究所 | 一种多旋翼无人机编队防撞控制方法 |
| CN114089780B (zh) * | 2022-01-21 | 2022-04-12 | 中国电子科技集团公司第二十八研究所 | 一种面向城市空间的多旋翼无人机路径规划方法 |
| CN114089780A (zh) * | 2022-01-21 | 2022-02-25 | 中国电子科技集团公司第二十八研究所 | 一种面向城市空间的多旋翼无人机路径规划方法 |
| CN117970952A (zh) * | 2024-03-28 | 2024-05-03 | 中国人民解放军海军航空大学 | 无人机机动策略离线建模方法 |
| CN117970952B (zh) * | 2024-03-28 | 2024-06-04 | 中国人民解放军海军航空大学 | 无人机机动策略离线建模方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112148024B (zh) | 2022-04-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112148024B (zh) | 基于自适应伪谱法的无人机实时在线航迹规划方法 | |
| US11727812B2 (en) | Airplane flight path planning method and device based on the pigeon-inspired optimization | |
| Huang et al. | Adaptive cylinder vector particle swarm optimization with differential evolution for UAV path planning | |
| Tisdale et al. | Autonomous UAV path planning and estimation | |
| CN109740198B (zh) | 一种基于解析预测的滑翔飞行器三维再入制导方法 | |
| Karimi et al. | Optimal maneuver-based motion planning over terrain and threats using a dynamic hybrid PSO algorithm | |
| Oh et al. | Decentralised standoff tracking of moving targets using adaptive sliding mode control for UAVs | |
| CN109871031B (zh) | 一种固定翼无人机的轨迹规划方法 | |
| CN111538241B (zh) | 一种平流层飞艇水平轨迹智能控制方法 | |
| CN111427379B (zh) | 观测驱动的多无人机协同standoff目标跟踪方法 | |
| Cai et al. | Path planning for UAV tracking target based on improved A-star algorithm | |
| CN110262555B (zh) | 连续障碍环境下无人机实时避障控制方法 | |
| CN112947572B (zh) | 一种基于地形跟随的四旋翼飞行器自适应运动规划方法 | |
| CN113268074B (zh) | 一种基于联合优化的无人机航迹规划方法 | |
| Chen et al. | Tracking with UAV using tangent-plus-Lyapunov vector field guidance | |
| Bin et al. | Research on cooperative collision avoidance problem of multiple UAV based on reinforcement learning | |
| Oh et al. | Coordinated standoff tracking of groups of moving targets using multiple UAVs | |
| Shu et al. | Trajectory prediction of UAV based on LSTM | |
| CN114637327A (zh) | 基于深度策略性梯度强化学习的在线轨迹生成制导方法 | |
| Liang et al. | Multi-UAV autonomous collision avoidance based on PPO-GIC algorithm with CNN–LSTM fusion network | |
| CN114217603A (zh) | 一种多无人船编队安全优化控制系统 | |
| Li et al. | A warm-started trajectory planner for fixed-wing unmanned aerial vehicle formation | |
| CN113674310A (zh) | 一种基于主动视觉感知的四旋翼无人机目标跟踪方法 | |
| CN114943168B (zh) | 一种水上浮桥组合方法及系统 | |
| CN115145295A (zh) | 一种动态环境下无人机在线自主航迹优化控制方法 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |