CN114594785A - 基于混合整数二阶锥优化的无人机避障实时轨迹规划方法 - Google Patents

基于混合整数二阶锥优化的无人机避障实时轨迹规划方法 Download PDF

Info

Publication number
CN114594785A
CN114594785A CN202210049148.9A CN202210049148A CN114594785A CN 114594785 A CN114594785 A CN 114594785A CN 202210049148 A CN202210049148 A CN 202210049148A CN 114594785 A CN114594785 A CN 114594785A
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
constraint
obstacle avoidance
optimization
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
Application number
CN202210049148.9A
Other languages
English (en)
Other versions
CN114594785B (zh
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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202210049148.9A priority Critical patent/CN114594785B/zh
Publication of CN114594785A publication Critical patent/CN114594785A/zh
Application granted granted Critical
Publication of CN114594785B publication Critical patent/CN114594785B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，属于航空航天技术领域。本发明实现方法为：构建最短时间的最优控制问题，将其转为固定时间最优控制问题；引入无量纲状态量θ对航向角的正切值进行替换，引入约束
Figure DDA0003473178460000011
将动力学与目标函数中的非凸项转至此约束中，定义变量u将非凸项转移至不等式约束|u|≤ωmaxδ3/V；引入整型变量ηj，使二维平面任意形状的避障约束成为线性约束；对产生的非凸不等式约束|u|≤ωmaxδ3/V进行线性化处理；迭代求解该混合整数二阶锥优化问题，实现无人机避障实时轨迹最优规划。本发明具有如下优点：(1)求解混合整数二阶锥优化问题，规划高效；(2)对障碍物的形状没有严格的限制；(3)能够实现时间最优；(4)规划可靠性高。

Description

基于混合整数二阶锥优化的无人机避障实时轨迹规划方法
技术领域
本发明涉及一种无人机避障实时轨迹规划方法，尤其涉及适用于考虑最短时间指标下的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，属于航空航天技术领域。
背景技术
随着自主控制、通信技术、和高性能材料的快速发展，无人机已经广泛地应用于各种任务，其中较为典型的领域有军事侦察、消防安全、野外救援等等。上述任务对无人机的自主决策以及飞行能力提出较高的要求。尤其针对自主飞行任务，无人机必须要具备在线避障的能力。所谓无人机避障能力是指无人机在飞行过程中能够快速重复地规划无碰撞的可行甚至是最优的飞行轨迹。这就要求无人机能够针对不同形状的障碍物规划出一条安全且具备一定最优性的飞行轨迹。通常，优化方法可用来解决上述轨迹规划问题。然而，受限于数学模型的构建，目前现有的研究与技术大都局限于针对椭圆或圆形障碍的无人机轨迹规划，这使得此类技术的应用具有局限性。为了解决二维任意形状避障问题，一种基于混合整数的线性规划方法(MILP)考虑将整数变量引入，并取得了良好的效果。然而，此类方法将会引入大量的整数变量，这使得整个优化问题的求解效率较低，通常难以满足实际应用时计算效率的要求。因此，本专利提出的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，不仅能够适用于针对二维任意形状障碍物的无人机轨迹规划，相比于MILP方法，还能够有效地减少整数变量的个数，进而满足无人机在线避障规划的任务需求。
在已发展的关于无人机轨迹规划方法中在先技术[1](参见：王祝, 刘莉,龙腾,等.基于罚函数序列凸规划的多无人机轨迹规划[J].航空学报,2016,37(010):3149-3158.)考虑了具有非线性运动约束和非凸路径约束的无人机轨迹规划问题，建立了圆形障碍物的数学表达，并使用序列凸优化算法对无人机轨迹规划问题求解，以更好地权衡最优性与时效性，提升无人机避障能力，但是仅考虑了圆形障碍物，使得方法应用具有局限性。
在先技术[2](参见：Maia M H,RKH
Figure RE-GDA0003605304550000021
On the use of mixed-integerlinear programming for predictive control with avoidance constraints[J].International Journal ofRobust&Nonlinear Control,2010, 19(7):822-828.)该方法引入整数变量构建了针对不规则障碍(多项式障碍物)的无人机轨迹规划问题，并取得了良好的避障效果，获得了一条安全且具有最优性的飞行轨迹。然而，该方法引入了过多的整数变量降低了问题求解效率，此外，考虑了线性无人机模型使得此研究的方法无法适用非线性无人机模型。
因此，对于无人机避障实时轨迹规划，不仅仅需要考虑二维任意形状障碍物的构建，还要能够减少问题中整型变量的个数，才能够提升优化问题的求解效率，进而使得该问题的应用场景有明显的扩展。
发明内容
本发明公开的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法要解决的技术问题是：在二维任意形状避障约束下，考虑无人机二维非线性运动模型，并基于混合整数二阶锥优化实现无人机避障实时轨迹规划；具有如下优点：(1)求解混合整数二阶锥优化问题，规划高效；(2)对障碍物的形状没有严格的限制；(3)能够实现时间最优；(4)规划可靠性高。
本发明的目的是通过下述技术方案实现的：
本发明公开的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，首先建立以无人机当前位置为原点的惯性坐标系；在以无人机当前位置为原点的惯性坐标系下建立无人机的运动学方程以及约束条件，并构建最短时间的最优控制问题；使用横坐标x作为自变量，将最短时间最优控制问题转为固定时间最优控制问题；并引入无量纲状态量
Figure RE-GDA0003605304550000022
对航向角的正切值进行替换，将动力学中的非线性集中到动力学方程的最后一项，并引入约束
Figure RE-GDA0003605304550000023
继续将动力学与目标函数中的非凸项转至此约束中，并定义变量u将非凸项转移至不等式约束|u|≤ωmaxδ3/V；通过引入整型变量ηj，使二维平面任意形状的避障约束成为包含整数变量的线性约束；对产生的非凸不等式约束 |u|≤ωmaxδ3/V进行线性化处理并得到混合整数二阶锥优化问题；迭代求解该混合整数二阶锥优化问题获得最短时间的最优控制问题的最优解，实现基于混合整数二阶锥优化实现无人机避障实时轨迹最优规划；当只求解一次混合整数二阶锥优化问题亦能够获得原问题的近似最优解，实现无人机避障实时轨迹规划。
本发明公开的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，包括如下步骤：
步骤一：建立以无人机当前位置为原点的惯性坐标系。
选择以无人机当前位置为原点O来建立惯性坐标系，x轴指向目标点，y轴与x轴构成右手直角坐标系，即完成以无人机当前位置为原点的惯性坐标系的建立。
步骤二：在步骤一建立的惯性坐标系下建立无人机的运动学方程以及约束条件，并构建最短时间的最优控制问题。所述约束条件包括控制约束、终端约束、二维平面任意形状的避障约束，通过集合的形式建立二维平面任意形状的避障约束，使本发明适用于二维平面任意形状障碍物规划。
步骤2.1：在步骤一建立的惯性坐标系下建立无人机的运动学方程；建立控制约束、终端约束，并通过集合的形式建立二维平面任意形状的避障约束，进而使本发明适用于二维平面任意形状障碍物规划。
无人机在该惯性坐标系下的运动学方程表示为：
Figure RE-GDA0003605304550000031
其中x表示无人机的横坐标位置，y表示无人机的纵坐标位置，θ代表无人机的航向角，V表示无人机的飞行速度(常数)，ω代表无人机的航向角的变化率，是控制量。
无人机的控制约束如下：
|ω|≤ωmax (2)
其中，ωmax表示所允许的最大航向角速率。
使用集合的形式表示无人机的避障约束如下：
Figure RE-GDA0003605304550000041
其中，X表示整个二维平面空间，Xo,j表示被第j个障碍物占据的平面空间，M表示障碍物的总个数。
无人机的终端约束如下：
x(tf)＝xf,y(tf)＝yf,θ(tf)＝θf (4)
其中，tf表示无人机的飞行时间，xf、yfθf分别表示给定的终端横坐标、纵坐标以及航向角。
步骤2.2：建立最短时间的最优控制问题。
考虑时间最短的优化指标，得到如下最优控制问题
Figure RE-GDA0003605304550000042
其中，tf为飞行时间是需要优化求解的变量。
步骤三：使用横坐标x作为自变量，将步骤二建立的最短时间最优控制问题转为固定时间最优控制问题，便于离散化处理；并引入无量纲状态量
Figure RE-GDA0003605304550000043
对航向角的正切值进行替换，将动力学中的非线性集中到动力学方程的最后一项，并引入约束
Figure RE-GDA0003605304550000044
继续将动力学与目标函数中的非凸项转至此约束中，并定义变量u将非凸项转移至不等式约束|u|≤ωmaxδ3/V，方便后续步骤六凸化，提高优化效率。
步骤3.1：使用横坐标x作为自变量，将步骤二建立的最短时间最优控制问题转为固定时间最优控制问题，便于离散化处理。
在最短时间问题以及步骤一建立的惯性坐标系下，横坐标x是单调递增的。选择横坐标x作为自变量，运动学方程变为：
Figure RE-GDA0003605304550000051
其中，y′和θ′表示变量y和θ对横坐标x的导数。式的形式不发生变化。
式中的避障约束变为：
Figure RE-GDA0003605304550000052
其中，
Figure RE-GDA0003605304550000053
并且
Figure RE-GDA0003605304550000054
表示集合Xo,j的边界值。式变为：
y(xf)＝yf,θ(xf)＝θf (8)
式中的优化目标变为：
Figure RE-GDA0003605304550000055
步骤3.2：引入无量纲状态量
Figure RE-GDA0003605304550000056
对航向角的正切值进行替换，将动力学中的非线性集中到动力学方程的最后一项，并引入约束
Figure RE-GDA0003605304550000057
继续将动力学与目标函数中的非凸项转至此约束中，并定义变量u将非凸项转移至不等式约束|u|≤ωmaxδ3/V，方便后续步骤六凸化，提高优化效率。
通过公式(10)引入无量纲状态量
Figure RE-GDA0003605304550000058
对航向角的正切值进行替换：
Figure RE-GDA0003605304550000059
利用式(10)，式中运动学方程变为：
Figure RE-GDA00036053045500000510
此时，原运动学中的状态θ已经被
Figure RE-GDA00036053045500000511
替换掉，将动力学中的非线性集中到动力学方程的最后一项，式中的目标函数变为：
Figure RE-GDA00036053045500000512
通过公式(13)引入约束
Figure RE-GDA0003605304550000061
将动力学与目标函数中的非凸项转至过程此约束：
Figure RE-GDA0003605304550000062
此时式中运动学变为：
Figure RE-GDA0003605304550000063
而式中的优化目标变为下列线性形式：
Figure RE-GDA0003605304550000064
通过公式定义变量u如下所示：
u:＝δ3ω/V (16)
利用式，式中的动力学变为下列线性形式：
Figure RE-GDA0003605304550000065
通过式，式中的非凸项被转移至式中
|u|≤ωmaxδ3/V (18)
而式中的终端约束变为
Figure RE-GDA0003605304550000066
步骤四：在步骤三的使用横坐标x作为自变量后，通过引入整型变量ηj，使步骤二中的二维平面任意形状的避障约束成为包含整数变量的线性约束，进一步提高规划求解效率。
对于任意一个障碍物j，通过引入整数变量ηj，二维平面任意形状的避障约束式变为线性约束(20)，进一步提高规划求解效率。
Figure RE-GDA0003605304550000067
其中，D是一个足够大的常数。
步骤五：对步骤三中的过程约束
Figure RE-GDA0003605304550000068
进行松弛处理，得到约束
Figure RE-GDA0003605304550000071
的凸化形式，并结合步骤三、步骤四得到最短时间的最优控制问题的松弛形式，提高规划求解效率。
对步骤三中的过程约束
Figure RE-GDA0003605304550000072
式进行松弛处理，式被松弛为如式(22)所示的凸化形式：
Figure RE-GDA0003605304550000073
式是凸的，即得到松弛问题如下：
Figure RE-GDA0003605304550000074
步骤六：对步骤三中产生的非凸不等式约束|u|≤ωmaxδ3/V进行线性化处理，离散化后得到混合整数二阶锥优化问题；迭代求解该混合整数二阶锥优化问题获得最短时间的最优控制问题的最优解，实现基于混合整数二阶锥优化实现无人机避障实时轨迹最优规划；不同于常规凸优化方法在未收敛之前无法产生可行解，若只求解一次混合整数二阶锥优化问题亦能够获得原问题的近似最优解，实现无人机避障实时轨迹规划，能够用于提高规划效率。
对步骤三中产生的非凸不等式约束|u|≤ωmaxδ3/V进行线性化处理，在给定的初值δ(0)后，式能够被线性化为：
Figure RE-GDA0003605304550000075
离散点的数量为(N+1)，在本问题中的优化变量包括： y＝[y0y1...yN]T，
Figure RE-GDA0003605304550000076
u＝[u0u1...uN]T，δ＝[δ0δ1...δN]T，η＝[η0η1...ηM]T。我们定义变量
Figure RE-GDA0003605304550000077
离散化后得到混合整数二阶锥优化问题如下：
Figure RE-GDA0003605304550000081
其中，c，p和b是带有恰当维度的向量，Θ和H是带有恰当维度的矩阵，K是二阶锥的笛卡尔乘积。gm代表式中的约束。问题 Problem D(δ(0))是一个混合整数二阶锥优化问题，给定一个初始的剖面δ(0)，迭代求解该混合整数二阶锥优化问题获得最短时间的最优控制问题的最优解，实现基于混合整数二阶锥优化实现无人机避障实时轨迹最优规划；不同于常规凸优化方法在未收敛之前无法产生可行解，若只求解一次混合整数二阶锥优化问题亦能够获得原问题的近似最优解，实现无人机避障实时轨迹规划。
有益效果：
1、本发明公开的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，使用横坐标x作为自变量，将建立的最短时间最优控制问题转为固定时间最优控制问题，便于离散化处理；并引入无量纲状态量
Figure RE-GDA0003605304550000082
对航向角的正切值进行替换，将动力学中的非线性集中到动力学方程的最后一项，并引入约束
Figure RE-GDA0003605304550000083
继续将动力学与目标函数中的非凸项转至此约束中，并定义变量u将非凸项转移至不等式约束 |u|≤ωmaxδ3/V，方便凸化处理，对过程约束
Figure RE-GDA0003605304550000084
进行松弛处理，得到约束
Figure RE-GDA0003605304550000085
的凸化形式，提高求解效率。
2、本发明公开的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，在使用横坐标x作为自变量后，通过引入整型变量ηj，使二维平面任意形状的避障约束成为包含整数变量的线性约束，进一步提高规划求解效率。
3、本发明公开的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，在建立约束条件过程中，通过集合形式建立二维平面任意形状的避障约束，使本发明适用于二维平面任意形状障碍物规划。
4、本发明公开的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，不同于常规凸优化方法在未收敛之前无法产生可行解，若只求解一次混合整数二阶锥优化问题亦能够获得原问题的近似最优解，实现无人机避障实时轨迹规划，能够用于提高规划效率与可靠性。
附图说明
图1是本发明步骤1中无人机坐标系建立的示意图；
图2是本发明基于混合整数二阶锥优化的无人机避障实时轨迹规划方法的流程图；
图3是本实施例中迭代求解和只求解一次时无人机的飞行轨迹；
图4是本实施例中迭代求解和只求解一次时无人机的航向角的变化曲线。
图5是本实施例中迭代求解和只求解一次时无人机的航向角速度的变化曲线。
具体实施方式
为了更好地说明本发明的目的和优点，下面通过对基于混合整数二阶锥优化的无人机避障实时轨迹规划方法进行仿真分析，对本发明做出详细解释。
实施例1：
如图2所示，本实施例公开的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，包括如下步骤：
步骤一：建立以无人机当前位置为原点的惯性坐标系。
选择以无人机当前位置为原点O来建立惯性坐标系，x轴指向目标点，y轴与x轴构成右手直角坐标系，即完成以无人机当前位置为原点的惯性坐标系的建立。
步骤二：在步骤一建立的惯性坐标系下建立无人机的运动学方程以及约束条件，并构建最短时间的最优控制问题。所述约束条件包括控制约束、终端约束、二维平面任意形状的避障约束，通过集合的形式建立二维平面任意形状的避障约束，使本发明适用于二维平面任意形状障碍物规划。
步骤2.1：在步骤一建立的惯性坐标系下建立无人机的运动学方程；建立控制约束、终端约束，并通过集合的形式建立二维平面任意形状的避障约束，进而使本发明适用于二维平面任意形状障碍物规划。
无人机在该惯性坐标系下的运动学方程表示为：
Figure RE-GDA0003605304550000101
其中x表示无人机的横坐标位置，y表示无人机的纵坐标位置，θ代表无人机的航向角，V表示无人机的飞行速度(常数)，ω代表无人机的航向角的变化率，是控制量。
无人机的控制约束如下：
|ω|≤ωmax (26)
其中，ωmax表示所允许的最大航向角速率。
使用集合的形式表示无人机的避障约束如下：
Figure RE-GDA0003605304550000102
其中，X表示整个二维平面空间，Xo,j表示被第j个障碍物占据的平面空间，M表示障碍物的总个数。
无人机的终端约束如下：
x(tf)＝xf,y(tf)＝yf,θ(tf)＝θf (28)
其中，tf表示无人机的飞行时间，xf、yfθf分别表示给定的终端横坐标、纵坐标以及航向角。
步骤2.2：建立最短时间的最优控制问题。
考虑时间最短的优化指标，可得到如下最优控制问题
Figure RE-GDA0003605304550000111
其中，tf为飞行时间是需要优化求解的变量。
步骤三：使用横坐标x作为自变量，将步骤二建立的最短时间最优控制问题转为固定时间最优控制问题，便于离散化处理；并引入无量纲状态量
Figure RE-GDA0003605304550000112
对航向角的正切值进行替换，将动力学中的非线性集中到动力学方程的最后一项，并引入约束
Figure RE-GDA0003605304550000113
继续将动力学与目标函数中的非凸项转至此约束中，并定义变量u将非凸项转移至不等式约束|u|≤ωmaxδ3/V，方便后续步骤六凸化，提高求解效率。
步骤3.1：使用横坐标x作为自变量，将步骤二建立的最短时间最优控制问题转为固定时间最优控制问题，便于离散化处理。
在最短时间问题以及步骤一建立的惯性坐标系下，横坐标x是单调递增的。选择横坐标x作为自变量，运动学方程变为：
Figure RE-GDA0003605304550000114
其中，y′和θ′表示变量y和θ对横坐标x的导数。式的形式不发生变化。式中的避障约束变为：
Figure RE-GDA0003605304550000115
其中，
Figure RE-GDA0003605304550000116
并且
Figure RE-GDA0003605304550000117
表示集合Xo,j的边界值。式变为：
y(xf)＝yf,θ(xf)＝θf (32)
式中的优化目标变为：
Figure RE-GDA0003605304550000121
步骤3.2：引入无量纲状态量
Figure RE-GDA0003605304550000122
对航向角的正切值进行替换，将动力学中的非线性集中到动力学方程的最后一项，并引入约束
Figure RE-GDA0003605304550000123
继续将动力学与目标函数中的非凸项转至此约束中，并定义变量u将非凸项转移至不等式约束|u|≤ωmaxδ3/V，方便后续步骤六凸化，提高求解效率。
通过公式引入无量纲状态量
Figure RE-GDA0003605304550000124
对航向角的正切值进行替换：
Figure RE-GDA0003605304550000125
利用式，式中运动学方程变为：
Figure RE-GDA0003605304550000126
此时，原运动学中的状态θ已经被
Figure RE-GDA0003605304550000127
替换掉，将动力学中的非线性集中到动力学方程的最后一项，式中的目标函数变为：
Figure RE-GDA0003605304550000128
通过公式引入约束
Figure RE-GDA0003605304550000129
将动力学与目标函数中的非凸项转至过程此约束：
Figure RE-GDA00036053045500001210
此时式中运动学变为：
Figure RE-GDA00036053045500001211
而式中的优化目标变为下列线性形式：
Figure RE-GDA00036053045500001212
通过公式定义变量u如下所示：
u:＝δ3ω/V (40)
利用式，式中的动力学变为下列线性形式：
Figure RE-GDA0003605304550000131
通过式，式中的非凸项被转移至式中：
|u|≤ωmaxδ3/V (42)
而式中的终端约束变为：
Figure RE-GDA0003605304550000132
步骤四：在步骤三的使用横坐标x作为自变量后，通过引入整型变量ηj，使步骤二中的二维平面任意形状的避障约束成为包含整数变量的线性约束，进一步提高规划求解效率。
对于任意一个障碍物j，通过引入整数变量ηj，二维平面任意形状的避障约束式变为线性约束，进一步提高规划求解效率。
Figure RE-GDA0003605304550000133
其中，D是一个足够大的常数。
步骤五：对步骤三中的过程约束
Figure RE-GDA0003605304550000134
进行松弛处理，得到约束
Figure RE-GDA0003605304550000135
的凸化形式，并结合步骤三、步骤四得到最短时间的最优控制问题的松弛形式，提高规划求解效率。
对步骤三中的过程约束
Figure RE-GDA0003605304550000136
式进行松弛处理，式被松弛为如式所示的凸化形式：
Figure RE-GDA0003605304550000137
式是凸的，即得到松弛问题如下：
Figure RE-GDA0003605304550000138
步骤六：对步骤三中产生的非凸不等式约束|u|≤ωmaxδ3/V进行线性化处理，离散化后得到混合整数二阶锥优化问题；迭代求解该混合整数二阶锥优化问题获得最短时间的最优控制问题的最优解，实现基于混合整数二阶锥优化实现无人机避障实时轨迹最优规划；不同于常规凸优化方法在未收敛之前无法产生可行解，若只求解一次混合整数二阶锥优化问题亦能够获得原问题的近似最优解，实现无人机避障实时轨迹规划，能够用于提高规划效率。
对步骤三中产生的非凸不等式约束|u|≤ωmaxδ3/V进行线性化处理，在给定的初值δ(0)后，式能够被线性化为：
Figure RE-GDA0003605304550000141
离散点的数量为(N+1)，在本问题中的优化变量包括： y＝[y0y1...yN]T，
Figure RE-GDA0003605304550000142
u＝[u0u1...uN]T，δ＝[δ0δ1...δN]T，η＝[η0η1...ηM]T。我们定义变量
Figure RE-GDA0003605304550000143
离散化后得到混合整数二阶锥优化问题如下：
Figure RE-GDA0003605304550000144
其中，c，p和b是带有恰当维度的向量，Θ和H是带有恰当维度的矩阵，K是二阶锥的笛卡尔乘积。gm代表式中的约束。问题 Problem D(δ(0))是一个混合整数二阶锥优化问题，给定一个初始的剖面δ(0)，迭代求解该混合整数二阶锥优化问题获得最短时间的最优控制问题的最优解，实现基于混合整数二阶锥优化实现无人机避障实时轨迹最优规划；不同于常规凸优化方法在未收敛之前无法产生可行解，若只求解一次混合整数二阶锥优化问题亦能够获得原问题的近似最优解，实现无人机避障实时轨迹规划
为了验证方法的可行性，选择无人机的初始位置为(0,0)m，终端位置为
Figure RE-GDA0003605304550000145
给定航向角变化率的限制为20°/s，具体数据如表 1所示。
表1无人机初始化设置
数值
初始位置(x<sub>0</sub>,y<sub>0</sub>) (0,0)m
初始航向角θ<sub>0</sub> 0°
终端位置(x<sub>f</sub>,y<sub>f</sub>) (110,0)m
终端航向角θ<sub>f</sub> 0°
航向角速度的限制ω<sub>max</sub> 20°/s
在本技术中的步骤六中提到迭代求解该问题Problem D(δ(0))即可获得原最优控制问题的解，若只求解一次该问题可获得原问题的近似最优解。为了验证本方法在迭代求解的最优性以及只求解一次的计算效率等方面的优势，本方法设置了上述两种求解方式的仿真对比分析。
使用表1中无人机的初始化设置，利用迭代求解和只求解一次得到了如图3和图4的仿真结果，图3展示了无人机在两种求解方式下的飞行轨迹，可以看到无论是哪种求解方式，无人机均能够有效地完成避障，并成功地到达指定的终端状态，这验证了此发明的有效性。此外，图4和图5分别展示了无人机的航向角以及航向角速度的变化曲线，可以看到求解一次得到的解的确是近似最优解，只有航向角速度在饱和处存在些许差异。这验证了本发明两种求解方式的正确性。此外，表2统计了迭代求解和只求解一次得到的飞行时间，计算时间以及迭代次数，可见只求解一次所耗时间最少，计算效率最高。若用户想要获得最优解可采用迭代方式求解，若想要提高求解效率，可以只求解一次即可获得近似最优解。
表2迭代求解和只求解一次得到的飞行时间，计算时间以及迭代次数
迭代求解 只求解一次
飞行时间t<sub>f</sub>(s) 23.95 23.98
计算时间t<sub>c</sub>(s) 0.652 0.151
迭代次数 3 1
以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例，用于解释本发明，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，其特征在于：包括如下步骤，
步骤一：建立以无人机当前位置为原点的惯性坐标系；
步骤二：在步骤一建立的惯性坐标系下建立无人机的运动学方程以及约束条件，并构建最短时间的最优控制问题；所述约束条件包括控制约束、终端约束、二维平面任意形状的避障约束，通过集合的形式建立二维平面任意形状的避障约束；
步骤三：使用横坐标x作为自变量，将步骤二建立的最短时间最优控制问题转为固定时间最优控制问题，便于离散化处理；并引入无量纲状态量θ对航向角的正切值进行替换，将动力学中的非线性集中到动力学方程的最后一项，并引入约束
Figure RE-FDA0003605304540000011
继续将动力学与目标函数中的非凸项转至此约束中，并定义变量u将非凸项转移至不等式约束|u|≤ωmaxδ3/V，方便后续步骤六凸化，提高优化效率；
步骤四：在步骤三的使用横坐标x作为自变量后，通过引入整型变量ηj，使步骤二中的二维平面任意形状的避障约束成为包含整数变量的线性约束，进一步提高规划求解效率；
步骤五：对步骤三中的过程约束
Figure RE-FDA0003605304540000012
进行松弛处理，得到约束
Figure RE-FDA0003605304540000013
的凸化形式，并结合步骤三、步骤四得到最短时间的最优控制问题的松弛形式，提高规划求解效率；
步骤六：对步骤三中产生的非凸不等式约束|u|≤ωmaxδ3/V进行线性化处理，离散化后得到混合整数二阶锥优化问题；迭代求解该混合整数二阶锥优化问题获得最短时间的最优控制问题的最优解，实现基于混合整数二阶锥优化实现无人机避障实时轨迹最优规划；不同于常规凸优化方法在未收敛之前无法产生可行解，若只求解一次混合整数二阶锥优化问题亦能够获得原问题的近似最优解，实现无人机避障实时轨迹规划，能够用于提高规划效率。
2.如权利要求1所述的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，其特征在于：步骤一实现方法为，
选择以无人机当前位置为原点O来建立惯性坐标系，x轴指向目标点，y轴与x轴构成右手直角坐标系，即完成以无人机当前位置为原点的惯性坐标系的建立。
3.如权利要求2所述的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，其特征在于：步骤二实现方法为，
步骤2.1：在步骤一建立的惯性坐标系下建立无人机的运动学方程；建立控制约束、终端约束，并通过集合的形式建立二维平面任意形状的避障约束，进而使本发明适用于二维平面任意形状障碍物规划；
无人机在该惯性坐标系下的运动学方程表示为：
Figure RE-FDA0003605304540000021
其中x表示无人机的横坐标位置，y表示无人机的纵坐标位置，θ代表无人机的航向角，V表示无人机的飞行速度，ω代表无人机的航向角的变化率，是控制量；
无人机的控制约束如下：
|ω|≤ωmax (2)
其中，ωmax表示所允许的最大航向角速率；
使用集合的形式表示无人机的避障约束如下：
Figure RE-FDA0003605304540000022
其中，X表示整个二维平面空间，Xo,j表示被第j个障碍物占据的平面空间，M表示障碍物的总个数；
无人机的终端约束如下：
x(tf)＝xf,y(tf)＝yf,θ(tf)＝θf (4)
其中，tf表示无人机的飞行时间，xf、yfθf分别表示给定的终端横坐标、纵坐标以及航向角；
步骤2.2：建立最短时间的最优控制问题；
考虑时间最短的优化指标，得到如下最优控制问题
Figure RE-FDA0003605304540000031
其中，tf为飞行时间是需要优化求解的变量。
4.如权利要求3所述的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，其特征在于：步骤三实现方法为，
步骤3.1：使用横坐标x作为自变量，将步骤二建立的最短时间最优控制问题转为固定时间最优控制问题，便于离散化处理；
在最短时间问题以及步骤一建立的惯性坐标系下，横坐标x是单调递增的；选择横坐标x作为自变量，运动学方程变为：
Figure RE-FDA0003605304540000032
其中，y′和θ′表示变量y和θ对横坐标x的导数；式的形式不发生变化；式中的避障约束变为：
Figure RE-FDA0003605304540000033
其中，
Figure RE-FDA0003605304540000034
并且
Figure RE-FDA0003605304540000035
表示集合Xo,j的边界值；式变为：
y(xf)＝yf,θ(xf)＝θf (8)
式中的优化目标变为：
Figure RE-FDA0003605304540000036
步骤3.2：引入无量纲状态量
Figure RE-FDA0003605304540000037
对航向角的正切值进行替换，将动力学中的非线性集中到动力学方程的最后一项，并引入约束
Figure RE-FDA0003605304540000041
继续将动力学与目标函数中的非凸项转至此约束中，并定义变量u将非凸项转移至不等式约束|u|≤ωmaxδ3/V，方便后续步骤六凸化，提高优化效率；
通过公式(10)引入无量纲状态量
Figure RE-FDA0003605304540000042
对航向角的正切值进行替换：
Figure RE-FDA0003605304540000043
利用式(10)，式中运动学方程变为：
Figure RE-FDA0003605304540000044
此时，原运动学中的状态θ已经被
Figure RE-FDA0003605304540000045
替换掉，将动力学中的非线性集中到动力学方程的最后一项，式中的目标函数变为：
Figure RE-FDA0003605304540000046
通过公式(13)引入约束
Figure RE-FDA0003605304540000047
将动力学与目标函数中的非凸项转至过程此约束：
Figure RE-FDA0003605304540000048
此时式中运动学变为：
Figure RE-FDA0003605304540000049
而式中的优化目标变为下列线性形式：
Figure RE-FDA00036053045400000410
通过公式定义变量u如下所示：
u:＝δ3ω/V (16)
利用式，式中的动力学变为下列线性形式：
Figure RE-FDA00036053045400000411
通过式，式中的非凸项被转移至式中
|u|≤ωmaxδ3/V (18)
而式中的终端约束变为
Figure RE-FDA0003605304540000051
5.如权利要求4所述的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，其特征在于：步骤四实现方法为，
对于任意一个障碍物j，通过引入整数变量ηj，二维平面任意形状的避障约束式变为线性约束(20)，进一步提高规划求解效率；
Figure RE-FDA0003605304540000052
其中，D是一个足够大的常数。
6.如权利要求5所述的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，其特征在于：步骤五实现方法为，
对步骤三中的过程约束
Figure RE-FDA0003605304540000053
式进行松弛处理，式被松弛为如式(22)所示的凸化形式：
Figure RE-FDA0003605304540000054
式是凸的，即得到松弛问题如下：
Figure RE-FDA0003605304540000055
7.如权利要求6所述的基于混合整数二阶锥优化的无人机避障实时轨迹规划方法，其特征在于：步骤六实现方法为，
对步骤三中产生的非凸不等式约束|u|≤ωmaxδ3/V进行线性化处理，在给定的初值δ(0)后，式能够被线性化为：
Figure RE-FDA0003605304540000056
离散点的数量为(N+1)，在本问题中的优化变量包括：y＝[y0y1...yN]T，
Figure RE-FDA0003605304540000057
u＝[u0u1...uN]T，δ＝[δ0δ1...δN]T，η＝[η0η1...ηM]T；我们定义变量
Figure RE-FDA0003605304540000061
离散化后得到混合整数二阶锥优化问题如下：
Figure RE-FDA0003605304540000062
其中，c，p和b是带有恰当维度的向量，Θ和H是带有恰当维度的矩阵，K是二阶锥的笛卡尔乘积；gm代表式中的约束；问题Problem D(δ(0))是一个混合整数二阶锥优化问题，给定一个初始的剖面δ(0)，迭代求解该混合整数二阶锥优化问题获得最短时间的最优控制问题的最优解，实现基于混合整数二阶锥优化实现无人机避障实时轨迹最优规划；不同于常规凸优化方法在未收敛之前无法产生可行解，若只求解一次混合整数二阶锥优化问题亦能够获得原问题的近似最优解，实现无人机避障实时轨迹规划。
CN202210049148.9A 2022-01-17 2022-01-17 基于混合整数二阶锥优化的无人机避障实时轨迹规划方法 Active CN114594785B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210049148.9A CN114594785B (zh) 2022-01-17 2022-01-17 基于混合整数二阶锥优化的无人机避障实时轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210049148.9A CN114594785B (zh) 2022-01-17 2022-01-17 基于混合整数二阶锥优化的无人机避障实时轨迹规划方法

Publications (2)

Publication Number Publication Date
CN114594785A true CN114594785A (zh) 2022-06-07
CN114594785B CN114594785B (zh) 2023-07-14

Family

ID=81804780

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210049148.9A Active CN114594785B (zh) 2022-01-17 2022-01-17 基于混合整数二阶锥优化的无人机避障实时轨迹规划方法

Country Status (1)

Country Link
CN (1) CN114594785B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117806333A (zh) * 2024-02-29 2024-04-02 北京大学 复杂环境中保持可见性的目标追踪轨迹规划方法及装置

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR2848661A1 (fr) * 2002-12-13 2004-06-18 Thales Sa Equipement anticollision terrain embarque a bord d'aeronef avec aide au retour en vol normal
CN106842926A (zh) * 2017-02-08 2017-06-13 北京航空航天大学 一种基于正实b样条的飞行器轨迹优化方法
DE102015224796A1 (de) * 2015-12-10 2017-06-14 Robert Bosch Gmbh Verfahren und Steuergerät zum Erkennen einer möglichen Kollision eines unbemannten Luftfahrzeugs mit einem Objekt
CN108388270A (zh) * 2018-03-21 2018-08-10 天津大学 面向安全域的集群无人机轨迹姿态协同控制方法
CN108445898A (zh) * 2018-05-14 2018-08-24 南开大学 基于微分平坦特性的四旋翼无人飞行器***运动规划方法
CN109976154A (zh) * 2019-03-04 2019-07-05 北京理工大学 一种基于混沌多项式和序列凸优化的飞行器轨迹优化方法
CN111562797A (zh) * 2020-07-06 2020-08-21 北京理工大学 确保收敛的无人机飞行时间最优实时轨迹优化方法
CN111722641A (zh) * 2020-04-10 2020-09-29 南京非空航空科技有限公司 一种微型无人机高机动轨迹规划方法
CN113157001A (zh) * 2021-05-25 2021-07-23 北京航空航天大学 一种基于二阶锥优化的无人机路径规划方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR2848661A1 (fr) * 2002-12-13 2004-06-18 Thales Sa Equipement anticollision terrain embarque a bord d'aeronef avec aide au retour en vol normal
DE102015224796A1 (de) * 2015-12-10 2017-06-14 Robert Bosch Gmbh Verfahren und Steuergerät zum Erkennen einer möglichen Kollision eines unbemannten Luftfahrzeugs mit einem Objekt
CN106842926A (zh) * 2017-02-08 2017-06-13 北京航空航天大学 一种基于正实b样条的飞行器轨迹优化方法
CN108388270A (zh) * 2018-03-21 2018-08-10 天津大学 面向安全域的集群无人机轨迹姿态协同控制方法
CN108445898A (zh) * 2018-05-14 2018-08-24 南开大学 基于微分平坦特性的四旋翼无人飞行器***运动规划方法
CN109976154A (zh) * 2019-03-04 2019-07-05 北京理工大学 一种基于混沌多项式和序列凸优化的飞行器轨迹优化方法
CN111722641A (zh) * 2020-04-10 2020-09-29 南京非空航空科技有限公司 一种微型无人机高机动轨迹规划方法
CN111562797A (zh) * 2020-07-06 2020-08-21 北京理工大学 确保收敛的无人机飞行时间最优实时轨迹优化方法
CN113157001A (zh) * 2021-05-25 2021-07-23 北京航空航天大学 一种基于二阶锥优化的无人机路径规划方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
张洲宇: "基于局部方位信息的无人机避障轨迹规划", 《中国科学技术科学》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117806333A (zh) * 2024-02-29 2024-04-02 北京大学 复杂环境中保持可见性的目标追踪轨迹规划方法及装置
CN117806333B (zh) * 2024-02-29 2024-05-24 北京大学 复杂环境中保持可见性的目标追踪轨迹规划方法及装置

Also Published As

Publication number Publication date
CN114594785B (zh) 2023-07-14

Similar Documents

Publication Publication Date Title
Li et al. Robust tracking control strategy for a quadrotor using RPD-SMC and RISE
Wang et al. Robust H∞ attitude tracking control of a quadrotor UAV on SO (3) via variation-based linearization and interval matrix approach
CN109828600B (zh) 时间最优快速三维避障路径规划方法
CN108227491B (zh) 一种基于滑模神经网络的智能车轨迹跟踪控制方法
Kada et al. Robust PID controller design for an UAV flight control system
Lu et al. Online optimisation‐based backstepping control design with application to quadrotor
Xian et al. Robust tracking control of a quadrotor unmanned aerial vehicle-suspended payload system
Kim et al. Reinforcement learning-based optimal flat spin recovery for unmanned aerial vehicle
CN111766889A (zh) 基于输出反馈的四旋翼自适应动态面滑模控制器
CN111665855A (zh) 一种机器人预设性编队控制方法
Hafez et al. Unmanned aerial vehicles formation using learning based model predictive control
Benedikter et al. Convex approach to covariance control with application to stochastic low-thrust trajectory optimization
Gu et al. Trajectory planning and tracking control of a ground mobile robot: A reconstruction approach towards space vehicle
Yeh Attitude controller design of mini-unmanned aerial vehicles using fuzzy sliding-mode control degraded by white noise interference
CN115877871B (zh) 一种基于强化学习的非零和博弈无人机编队控制方法
CN111781833B (zh) 基于状态依赖分解的航天器在线最优姿态规避控制方法
Biglarbegian A novel robust leader-following control design for mobile robots
Jeong et al. Control System Design for a Ducted‐Fan Unmanned Aerial Vehicle Using Linear Quadratic Tracker
CN114594785A (zh) 基于混合整数二阶锥优化的无人机避障实时轨迹规划方法
Ma et al. Robust optimal control of connected and automated vehicle platoons through improved particle swarm optimization
Liu et al. Antisaturation fixed-time attitude tracking control based low-computation learning for uncertain quadrotor UAVs with external disturbances
CN110658811B (zh) 基于神经网络的受限移动机器人协同路径跟踪控制方法
Nguyen et al. Formation control scheme with reinforcement learning strategy for a group of multiple surface vehicles
Huang et al. Symmetry-Informed Reinforcement Learning and its Application to Low-Level Attitude Control of Quadrotors
Yu et al. Fast cooperative trajectory generation of unmanned aerial vehicles using a bezier curve-based shaping method

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