CN108120442B - 一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法 - Google Patents

一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法 Download PDF

Info

Publication number
CN108120442B
CN108120442B CN201711321646.XA CN201711321646A CN108120442B CN 108120442 B CN108120442 B CN 108120442B CN 201711321646 A CN201711321646 A CN 201711321646A CN 108120442 B CN108120442 B CN 108120442B
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
rotor unmanned
constraint
constraints
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.)
Expired - Fee Related
Application number
CN201711321646.XA
Other languages
English (en)
Other versions
CN108120442A (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 CN201711321646.XA priority Critical patent/CN108120442B/zh
Publication of CN108120442A publication Critical patent/CN108120442A/zh
Application granted granted Critical
Publication of CN108120442B publication Critical patent/CN108120442B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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

本发明公开的一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法，属于轨迹规划技术领域。本发明实现方法如下：针对多旋翼无人机轨迹生成问题，建立包括性能指标、运动学约束、状态约束、推力约束、倾斜角约束和障碍规避约束的最优控制问题模型；基于离散化方法和逐次线性化将最优控制问题转化为二阶锥规划问题，然后对二阶锥规划问题进行迭代求解，从而生成满足复杂约束的多旋翼无人机飞行轨迹。本发明要解决的技术问题为：根据实际任务需要，基于二阶锥规划方法获得满足复杂约束的多旋翼无人机飞行轨迹，具有轨迹生成效率高、轨迹结果最优性好的优点。

Description

一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法
技术领域
本发明涉及一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法，属于轨迹规划技术领域。
背景技术
无人机是一种由动力驱动，机上无人驾驶，可重复使用的航空器，它具有体积小、造价低、使用方便、环境要求低、生存能力较强等优点，非常适合执行枯燥、恶劣、危险的任务。无人机主要包括固定翼无人机、无人直升机、多旋翼无人机和其他类型无人机。其中，多旋翼无人机由于具有悬停精度高、机动性好、控制方式简单、鲁棒性强等优势，近年来在区域侦察、电力巡线、航空摄影等领域得到了广泛应用。
多旋翼无人机应用于侦察、巡线等专业领域时，由于飞行时间长、飞行距离远的任务特点，传统的遥控飞行方式无法保证任务的顺利完成，因此自主飞行成为多旋翼无人机执行任务的主要控制模式。首先规划生成一条可行的无人机飞行轨迹，然后利用飞控系统控制无人机跟踪规划轨迹，从而实现多旋翼无人机自主飞行。可见，多旋翼无人机飞行轨迹的生成是实现其自主飞行的前提。
多旋翼无人机轨迹生成需要考虑无人机飞行性能、障碍规避、允许控制等约束，以能源消耗最少或时间最短等为指标，为无人机生成一条可行的最优或次优轨迹。本质上，多旋翼无人机轨迹生成是一类典型的最优控制问题。最优控制问题的求解方法包括间接法和直接法两类。间接法基于极小值原理将最优控制问题转化为两点边值问题，通过求解两点边值问题间接获得最优控制问题的解。然而，两点边值问题求解时需要的初始猜测难以给出，而且对于含有复杂约束的实际问题，常常无法实现最优控制问题向两点边值问题的转化，因此间接法在实际中的应用较少。直接法则将最优控制问题参数化为非线性优化问题，然后利用非线性优化直接对性能指标寻优。直接法不需猜测协态变量的初值，而且具有收敛半径大，适用范围广的优点，已成为当前求解最优控制问题的主要方法。
对于无人机轨迹生成问题，利用直接法转化得到的参数优化问题一般具有高维和非凸特征。如果采用传统的非线性优化方法直接求解，需要消耗大量的计算时间，而且常常难以保证收敛性。对此，部分研究通过近似和线性化将问题转化为线性规划或二次规划问题，实现快速求解，获得问题的近似解。但是，线性规划或二次规划模型仅能描述简单问题，难以对无人机轨迹生成问题中的实际复杂约束进行准确描述。例如，多旋翼无人机执行区域侦察、电力巡线、航拍任务时，需要对无人机的倾斜角进行约束以保证机载传感器能够有效对准目标，该约束属于非线性约束，无法在线性规划或二次规划的框架下实现，然而该约束对多旋翼无人机顺利完成既定任务至关重要。因此，多旋翼无人机轨迹生成方法必须在考虑实际复杂约束的条件下快速获得可行轨迹。
发明内容
本发明公开的一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法要解决的技术问题为：根据实际任务需要，基于二阶锥规划方法获得满足复杂约束的多旋翼无人机飞行轨迹，具有轨迹生成效率高、轨迹结果最优性好的优点。所述的复杂约束包括运动学约束、状态约束、推力约束、倾斜角约束和障碍规避约束。
本发明的目的是通过下述技术方案实现的：
本发明公开的一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法，针对多旋翼无人机轨迹生成问题，建立包括性能指标、运动学约束、状态约束、推力约束、倾斜角约束和障碍规避约束的最优控制问题模型；基于离散化方法和逐次线性化将最优控制问题转化为二阶锥规划问题，然后对二阶锥规划问题进行迭代求解，从而生成满足复杂约束的多旋翼无人机飞行轨迹。
本发明公开的一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法，包括如下步骤：
步骤一：输入多旋翼无人机参数信息、轨迹约束信息、任务环境信息和算法参数信息。所述的多旋翼无人机参数信息包括多旋翼无人机的质量、最大推力。所述的轨迹约束信息包括初始位置、初始速度、目标位置、目标速度。所述的任务环境信息包括障碍、边界。所述的算法参数信息包括飞行时间离散区间的数量、位置收敛最大允许误差、速度收敛最大允许误差。
步骤二：针对多旋翼无人机轨迹生成问题，建立包括性能指标、运动学约束、状态约束、推力约束、倾斜角约束和障碍规避约束的最优控制问题模型。
步骤二具体实现方法如下：
多旋翼无人机轨迹生成的性能指标J的表达式如式(1)所示，通过最小化如式(1)所示的性能指标J以获得平滑的多旋翼无人机飞行轨迹。
Figure GDA0002791819150000031
其中，a为多旋翼螺旋桨推力产生的加速度，g为重力加速度，t0表示飞行轨迹的初始时刻，tf表示飞行轨迹的终端时刻，||·||2表示二范数。
多旋翼无人机的运动学约束表示为如公式(2)所示的线性微分方程组。
Figure GDA0002791819150000032
其中，t表示时间，p＝(px,py,pz)T表示无人机的位置，px、py、pz分别表示无人机位置在x、y、z轴上的分量，v＝(vx,vy,vz)T表示无人机的速度，vx、vy、vz分别表示无人机速度在x、y、z轴上的分量，a＝(ax,ay,az)T表示无人机的加速度，ax、ay、az分别表示无人机加速度在x、y、z轴上的分量。
多旋翼无人机的状态约束包括初始状态约束、终端状态约束和状态边界约束，分别如式(3)、(4)和(5)所示。
p(t0)＝p0,v(t0)＝v0 (3)
p(tf)＝pf,v(tf)＝vf (4)
pmin≤p(t)≤pmax,vmin≤v(t)≤vmax,t∈[t0,tf] (5)
其中，p0表示初始位置，v0表示初始速度，pf表示终端位置，vf表示终端速度，pmin表示位置下边界，pmax表示位置上边界，vmin表示速度下边界，vmax表示速度上边界。
多旋翼无人机的推力约束如式(6)所示，其中Fmax为所有螺旋桨共同产生的推力最大值，m为无人机质量，g表示重力加速度的大小。
||(ax(t),ay(t),az(t)+g)||2≤Fmax/m (6)
多旋翼无人机的倾斜角约束表示为式(7)所示，其中αmax为允许的最大倾斜角。
||(ax(t),ay(t),0)||2≤tanαmax·(az(t)+g) (7)
多旋翼无人机障碍规避约束表示如式(8)所示，其中，pxy＝(px,py)T表示无人机位置p在水平面的投影，
Figure GDA0002791819150000041
为第m个障碍的中心位置，
Figure GDA0002791819150000042
为第m个障碍的半径，M为障碍总数量。
Figure GDA0002791819150000043
根据式(1)给出的性能指标和式(2)-(8)给出的约束条件，建立针对多旋翼无人机轨迹生成问题的最优控制问题，具体如式(9)所示。
Figure GDA0002791819150000044
步骤三：将步骤三中建立的最优控制问题参数化为最优化问题，包括对性能指标和约束条件的离散化。
步骤三具体实现方法如下：
将飞行时间均匀离散为K个区间，所述的区间数量K为步骤一输入的算法参数之一，每个区间的时间步长为Δt＝(tf-t0)/K，则多旋翼无人机的飞行轨迹可用K+1个离散点近似描述。记离散点的时刻为tk＝t0+k·Δt，多旋翼无人机在tk时刻的位置、速度和加速度为p[k]＝p(tk)，v[k]＝v(tk)和a[k]＝a(tk)，其中k＝0,1,…,K。
根据离散化的轨迹曲线，将式(1)给出的性能指标J转化为如式(10)所示的离散化的性能指标J′。
Figure GDA0002791819150000045
根据梯形数值积分法，将多旋翼无人机运动学微分方程转化为关于离散时刻tk处位置p[k]、速度v[k]、加速度a[k]的代数方程组，如式(11)所示。
Figure GDA0002791819150000046
多旋翼无人机的初始状态约束、终端状态约束和状态边界约束分别离散化为式(12)、(13)和(14)所示。
p[0]＝p0,v[0]＝v0 (12)
p[K]＝pf,v[K]＝vf (13)
pmin≤p[k]≤pmax,vmin≤v[k]≤vmax,k＝0,1,K,K (14)
多旋翼无人机的推力约束和倾斜角约束分别离散为式(15)和(16)所示的不等式约束。
||(ax[k],ay[k],az[k]+g)||2≤Fmax/m,k＝0,1,K,K (15)
||(ax[k],ay[k],0)||2≤tanαmax·(az[k]+g),k＝0,1,K,K (16)
多旋翼无人机的障碍规避约束离散化后为式(17)所示的不等式约束。
Figure GDA0002791819150000051
至此，利用离散化方法实现对步骤二中最优控制问题的参数化，建立式(18)所示的最优化问题，其中的优化变量为离散时刻tk处的多旋翼无人机位置p[k]、速度v[k]、加速度a[k]。式(18)所示的最优化问题是非凸优化问题，障碍规避约束为非凸约束，推力约束和倾斜角约束为二阶锥约束，其余约束均为线性约束。
Figure GDA0002791819150000052
步骤四：利用逐次线性化方法，对障碍规避约束在基准轨迹附近进行线性化，从而将步骤三中建立的非凸优化问题转化为二阶锥规划问题。
步骤四具体实现方法如下：
式(18)建立的优化问题模型只有障碍规避约束不满足二阶锥规划的约束形式，因此基于基准轨迹对障碍规避约束进行线性化，得到如式(19)所示的线性化后的障碍规避约束。
Figure GDA0002791819150000053
其中，
Figure GDA0002791819150000054
为基准轨迹
Figure GDA0002791819150000055
位于tk时刻的位置在水平面的投影。
根据如式(19)所述的线性化后的障碍规避约束，结合式(11)-(16)所示的约束，建立如式(20)所示的二阶锥规划问题。
Figure GDA0002791819150000061
步骤五：建立不考虑障碍规避约束的二阶锥规划问题，如式(21)所示，求解如式(21)所示不考虑障碍规避约束的二阶锥规划问题，获得不考虑障碍情况下的多旋翼无人机飞行轨迹。由于不考虑障碍规避约束，因此求解该问题时无需提供基准轨迹。同时，将获得的不考虑障碍情况下多旋翼无人机飞行轨迹作为当前基准轨迹。
Figure GDA0002791819150000062
为了提高求解效率，步骤五所述的求解如式(21)所示不考虑障碍规避约束的二阶锥规划问题优选采用内点法求解。
步骤六：根据当前基准轨迹，对如式(21)所示的二阶锥规划问题进行求解，获得在当前基准轨迹处的满足复杂约束的多旋翼无人机飞行轨迹。
为了提高求解效率，步骤六所述的对如式(20)所示的二阶锥规划问题进行求解优选内点法。
步骤六所述的当前基准轨迹第一次输入由步骤五给出，以后的当前基准轨迹由步骤六、七迭代更新给出。
步骤七：判断步骤六获得的轨迹结果与当前基准轨迹间的误差是否满足收敛条件式(22)。若满足，则执行步骤八；若不满足，则将步骤六中得到的轨迹结果设为当前基准轨迹，继续执行步骤六。
Figure GDA0002791819150000063
其中，q表示迭代次数，εp为位置收敛最大允许误差，εv为速度收敛最大允许误差，pq[k]和vq[k]分别为第q次迭代得到的位置和速度，
Figure GDA0002791819150000064
和
Figure GDA0002791819150000065
分别为当前基准轨迹对应的位置和速度。
所述的轨迹结果指步骤六获得的满足复杂约束的多旋翼无人机飞行轨迹。
步骤八：输出步骤六得到的轨迹结果，即输出满足复杂约束的多旋翼无人机飞行轨迹。
有益效果
1、本发明公开的一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法，针对多旋翼无人机轨迹生成问题，建立包括性能指标、运动学约束、状态约束、推力约束、倾斜角约束和障碍规避约束的最优控制问题模型；基于离散化方法和逐次线性化将最优控制问题转化为二阶锥规划问题，然后对二阶锥规划问题进行迭代求解，从而生成满足复杂约束的多旋翼无人机飞行轨迹；具有轨迹生成效率高、飞行轨迹结果最优性好的优点。
2、本发明公开的一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法，为了提高求解效率，选内点法对如式(20)、(21)所示的二阶锥规划问题进行求解。
附图说明
图1为本发明公开的一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法流程图；
图2为具体实施实例中本发明方法获得的多旋翼无人机飞行轨迹；
图3为具体实施实例中本发明方法所得结果中倾斜角的变化曲线；
图4为具体实施实例中线性规划方法获得的多旋翼无人机飞行轨迹；
图5为具体实施实例中线性规划方法所得结果中倾斜角的变化曲线。
具体实施方式
为了更好的说明本发明的目的与优点，下面通过多旋翼无人机轨迹生成实例，结合附图与表格对本发明做进一步说明，并通过与基于线性规划的传统轨迹生成方法进行比较，对本发明的综合性能进行验证分析。
实施例1：
本实施例公开的一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法，具体实现步骤如下：
步骤一：输入多旋翼无人机参数信息、轨迹约束信息、任务环境信息和算法参数信息。
设定无人机质量为1.5kg，最大推力为30N，允许的最大倾斜角为20°，初始位置为(10m,3m,20m)T，初始速度为(0m/s,0m/s,0m/s)T，目标位置为(80m,40m,30m)T，目标速度为(0,0,0)T，无人机初始时刻为0s，终端时刻为10s，位置下边界为(0m,0m,0m)T，位置上边界为(100m,50m,100m)T，速度下边界为(-20m/s,-20m/s,-4m/s)T，速度上边界为(20m/s,20m/s,4m/s)T。环境中包含4个障碍，其位置分别为(30m,10m)T，(30m,30m)T，(60m,10m)T和(60m,30m)T，4个障碍的半径均为8m。同时，设定飞行时间离散区间的数量为50，位置收敛最大允许误差为0.01m，速度收敛最大允许误差为0.01m/s。
步骤二：根据上述具体实例的参数输入，建立针对该多旋翼无人机轨迹生成问题的最优控制问题，如式(23)-(30)所示。
Figure GDA0002791819150000081
Figure GDA0002791819150000082
p(0)＝(10m,3m,20m)T,v(0)＝(0m/s,0m/s,0m/s)T (25)
p(10s)＝(80m,40m,30m)T,v(10s)＝(0m/s,0m/s,0m/s)T (26)
Figure GDA0002791819150000083
||(ax(t),ay(t),az(t)+g)||2≤20m/s2 (28)
||(ax(t),ay(t),0)||2≤tan20°·(az(t)+g) (29)
Figure GDA0002791819150000084
步骤三：将步骤2中得到的最优控制问题参数化为最优化问题。针对该具体实例，参数化后的最优化问题如式(31)-(38)所示
Figure GDA0002791819150000085
Figure GDA0002791819150000086
p[0]＝p0,v[0]＝v0 (33)
p[50]＝pf,v[50]＝vf (34)
Figure GDA0002791819150000091
||(ax[k],ay[k],az[k]+g)||2≤20m/s2 (36)
||(ax[k],ay[k],0)||2≤tan20°·(az[k]+g) (37)
Figure GDA0002791819150000092
步骤四：对障碍规避约束在基准轨迹附近进行线性化，建立二阶锥规划问题。
记基准轨迹在每个离散点处为
Figure GDA0002791819150000093
则针对该具体实例中的障碍信息，可将4个障碍规避约束描述为如式(39)所示的线性约束。
Figure GDA0002791819150000094
从而，针对该具体实例，可建立如式(40)所示的二阶锥规划问题。
Figure GDA0002791819150000095
步骤五：建立如式(41)所示的不考虑障碍规避约束的二阶锥规划问题。采用内点法求解该问题，获得不考虑障碍情况下的多旋翼无人机飞行轨迹，将该轨迹作为当前基准轨迹。
Figure GDA0002791819150000101
步骤六：根据当前基准轨迹，采用内点法求解式(40)所示的二阶锥规划问题，获得当前基准轨迹下的多旋翼无人机飞行轨迹。
步骤七：判断步骤六求解得到的飞行轨迹与当前基准轨迹间的误差是否满足式(42)所示的收敛条件。若满足式(42)所示的收敛条件，则执行步骤八；若不满足式(42)所示的收敛条件，则将步骤六得到的飞行轨迹设为新的当前基准轨迹，并返回执行步骤六。
Figure GDA0002791819150000102
针对该具体实例，通过仿真计算，第1到7次执行步骤七时，不满足收敛条件，转步骤六；第8次执行步骤七时，满足收敛条件，执行步骤八。
步骤八：将步骤六得到的轨迹结果输出作为满足复杂约束的多旋翼无人机飞行轨迹。
针对上述具体实例，利用本实施例所述的基于二阶锥规划的多旋翼无人机轨迹生成方法得到的轨迹结果如图2所示。图2中，方形标识点为设定的无人机初始位置，星形标识点为无人机目标位置，圆形填充区域为障碍，从起始位置到目标位置的曲线即为基于二阶锥规划生成的多旋翼无人机飞行轨迹。由图2可知，利用本实施例所提出方法生成的轨迹能够引导四旋翼无人机从初始位置飞抵目标位置，并实现对环境中障碍的规避。图3给出了求解结果对应的倾斜角变化曲线。由图3可知，多旋翼无人机倾斜角的最大值保持在20°，满足倾斜角约束。
采用基于线性规划的传统轨迹生成方法得到的轨迹结果和对应的倾斜角曲线分别如图4和图5所示。由图4可知，尽管传统方法得到的轨迹结果能够规避障碍并引导无人机到达目标位置，但是由于线性规划方法无法对倾斜角约束进行准确建模，飞行过程中的倾斜角最大值超出20°，即不满足最大倾斜角约束(如图5所示)。
基于二阶锥规划的多旋翼无人机飞行轨迹生成方法得到的性能指标为1336.41，轨迹生成用时8.36s。基于线性规划的多旋翼无人机飞行轨迹生成方法得到的性能指标为1850.24，轨迹生成用时15.27s。对比两种轨迹生成方法得到的性能指标和算法用时可见，本实施例所述的一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法能够获得更优的飞行轨迹且轨迹生成的效率更高。
根据前述的多旋翼无人机轨迹生成实例仿真结果与分析可见，本实施例所述的基于二阶锥规划的多旋翼无人机轨迹生成方法能够为多旋翼无人机提供满足实际复杂约束的可行飞行轨迹，且轨迹结果相比传统方法具有更好的最优性，轨迹生成速度相比传统方法具有更高的效率，因此本发明具有很强的工程实用性，并且能够实现预期的发明目的。
以上的具体描述，是对发明的目的、技术方案和有益效果的进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施实例，仅用于解释本发明，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法，其特征在于：包括如下步骤，
步骤一：输入多旋翼无人机参数信息、轨迹约束信息、任务环境信息和算法参数信息；所述的多旋翼无人机参数信息包括多旋翼无人机的质量、最大推力；所述的轨迹约束信息包括初始位置、初始速度、目标位置、目标速度；所述的任务环境信息包括障碍、边界；所述的算法参数信息包括飞行时间离散区间的数量、位置收敛最大允许误差、速度收敛最大允许误差；
步骤二：针对多旋翼无人机轨迹生成问题，建立包括性能指标、运动学约束、状态约束、推力约束、倾斜角约束和障碍规避约束的最优控制问题模型；
多旋翼无人机轨迹生成的性能指标J的表达式如式(1)所示，通过最小化如式(1)所示的性能指标J以获得平滑的多旋翼无人机飞行轨迹；
Figure FDA0002791819140000011
其中，a为多旋翼螺旋桨推力产生的加速度，g为重力加速度，t0表示飞行轨迹的初始时刻，tf表示飞行轨迹的终端时刻，||·||2表示二范数；m为无人机的质量；
多旋翼无人机的运动学约束表示为如公式(2)所示的线性微分方程组；
Figure FDA0002791819140000012
其中，t表示时间，p＝(px,py,pz)T表示无人机的位置，px、py、pz分别表示无人机位置在x、y、z轴上的分量，v＝(vx,vy,vz)T表示无人机的速度，vx、vy、vz分别表示无人机速度在x、y、z轴上的分量，a＝(ax,ay,az)T表示无人机的加速度，ax、ay、az分别表示无人机加速度在x、y、z轴上的分量；
多旋翼无人机的状态约束包括初始状态约束、终端状态约束和状态边界约束，分别如式(3)、(4)和(5)所示；
p(t0)＝p0,v(t0)＝v0 (3)
p(tf)＝pf,v(tf)＝vf (4)
pmin≤p(t)≤pmax,vmin≤v(t)≤vmax,t∈[t0,tf] (5)
其中，p0表示初始位置，v0表示初始速度，pf表示终端位置，vf表示终端速度，pmin表示位置下边界，pmax表示位置上边界，vmin表示速度下边界，vmax表示速度上边界；
多旋翼无人机的推力约束如式(6)所示，其中Fmax为所有螺旋桨共同产生的推力最大值，m为无人机质量，g表示重力加速度的大小；
||(ax(t),ay(t),az(t)+g)||2≤Fmax/m (6)
多旋翼无人机的倾斜角约束表示为式(7)所示，其中αmax为允许的最大倾斜角；
||(ax(t),ay(t),0)||2≤tanαmax·(az(t)+g) (7)
多旋翼无人机障碍规避约束表示如式(8)所示，其中，pxy＝(px,py)T表示无人机位置p在水平面的投影，
Figure FDA0002791819140000021
为第m个障碍的中心位置，
Figure FDA0002791819140000022
为第m个障碍的半径，M为障碍总数量；
Figure FDA0002791819140000023
根据式(1)给出的性能指标和式(2)-(8)给出的约束条件，建立针对多旋翼无人机轨迹生成问题的最优控制问题，具体如式(9)所示；
Figure FDA0002791819140000024
步骤三：将步骤二中建立的最优控制问题参数化为最优化问题，包括对性能指标和约束条件的离散化；
将飞行时间均匀离散为K个区间，所述的区间数量K为步骤一输入的算法参数之一，每个区间的时间步长为Δt＝(tf-t0)/K，则多旋翼无人机的飞行轨迹可用K+1个离散点近似描述；记离散点的时刻为tk＝t0+k·Δt，多旋翼无人机在tk时刻的位置、速度和加速度为p[k]＝p(tk)，v[k]＝v(tk)和a[k]＝a(tk)，其中k＝0,1,…,K；
根据离散化的轨迹曲线，将式(1)给出的性能指标J转化为如式(10)所示的离散化的性能指标J′；
Figure FDA0002791819140000025
根据梯形数值积分法，将多旋翼无人机运动学微分方程转化为关于离散时刻tk处位置p[k]、速度v[k]、加速度a[k]的代数方程组，如式(11)所示；
Figure FDA0002791819140000031
多旋翼无人机的初始状态约束、终端状态约束和状态边界约束分别离散化为式(12)、(13)和(14)所示；
p[0]＝p0,v[0]＝v0 (12)
p[K]＝pf,v[K]＝vf (13)
pmin≤p[k]≤pmax,vmin≤v[k]≤vmax,k＝0,1,K,K (14)
多旋翼无人机的推力约束和倾斜角约束分别离散为式(15)和(16)所示的不等式约束；
||(ax[k],ay[k],az[k]+g)||2≤Fmax/m,k＝0,1,K,K (15)
||(ax[k],ay[k],0)||2≤tanαmax·(az[k]+g),k＝0,1,K,K (16)
多旋翼无人机的障碍规避约束离散化后为式(17)所示的不等式约束；
Figure FDA0002791819140000032
至此，利用离散化方法实现对步骤二中最优控制问题的参数化，建立式(18)所示的最优化问题，其中的优化变量为离散时刻tk处的多旋翼无人机位置p[k]、速度v[k]、加速度a[k]；式(18)所示的最优化问题是非凸优化问题，障碍规避约束为非凸约束，推力约束和倾斜角约束为二阶锥约束，其余约束均为线性约束；
Figure FDA0002791819140000033
步骤四：利用逐次线性化方法，对障碍规避约束在基准轨迹附近进行线性化，从而将步骤三中建立的非凸优化问题转化为二阶锥规划问题；
式(18)建立的优化问题模型只有障碍规避约束不满足二阶锥规划的约束形式，因此基于基准轨迹对障碍规避约束进行线性化，得到如式(19)所示的线性化后的障碍规避约束；
Figure FDA0002791819140000041
其中，
Figure FDA0002791819140000042
为基准轨迹
Figure FDA0002791819140000043
位于tk时刻的位置在水平面的投影；
根据如式(19)所述的线性化后的障碍规避约束，结合式(11)-(16)所示的约束，建立如式(20)所示的二阶锥规划问题；
Figure FDA0002791819140000044
步骤五：建立不考虑障碍规避约束的二阶锥规划问题，如式(21)所示，求解如式(21)所示不考虑障碍规避约束的二阶锥规划问题，获得不考虑障碍情况下的多旋翼无人机飞行轨迹；由于不考虑障碍规避约束，因此求解该问题时无需提供基准轨迹；同时，将获得的不考虑障碍情况下多旋翼无人机飞行轨迹作为当前基准轨迹；
Figure FDA0002791819140000045
步骤六：根据当前基准轨迹，对如式(21)所示的二阶锥规划问题进行求解，获得在当前基准轨迹处的满足复杂约束的多旋翼无人机飞行轨迹；
步骤六所述的当前基准轨迹第一次输入由步骤五给出，以后的当前基准轨迹由步骤六、七迭代更新给出；
步骤七：判断步骤六获得的轨迹结果与当前基准轨迹间的误差是否满足收敛条件式(22)；若满足，则执行步骤八；若不满足，则将步骤六中得到的轨迹结果设为当前基准轨迹，继续执行步骤六；
Figure FDA0002791819140000046
其中，q表示迭代次数，εp为位置收敛最大允许误差，εv为速度收敛最大允许误差，pq[k]和vq[k]分别为第q次迭代得到的位置和速度，
Figure FDA0002791819140000047
和
Figure FDA0002791819140000048
分别为当前基准轨迹对应的位置和速度；
所述的轨迹结果指步骤六获得的满足复杂约束的多旋翼无人机飞行轨迹；
步骤八：输出步骤六得到的轨迹结果，即输出满足复杂约束的多旋翼无人机飞行轨迹。
2.如权利要求1所述的一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法，其特征在于：
为了提高求解效率，步骤五所述的求解如式(21)所示不考虑障碍规避约束的二阶锥规划问题采用内点法求解；
为了提高求解效率，步骤六所述的对如式(20)所示的二阶锥规划问题进行求解选用内点法。
CN201711321646.XA 2017-12-12 2017-12-12 一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法 Expired - Fee Related CN108120442B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711321646.XA CN108120442B (zh) 2017-12-12 2017-12-12 一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711321646.XA CN108120442B (zh) 2017-12-12 2017-12-12 一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法

Publications (2)

Publication Number Publication Date
CN108120442A CN108120442A (zh) 2018-06-05
CN108120442B true CN108120442B (zh) 2021-02-12

Family

ID=62229002

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711321646.XA Expired - Fee Related CN108120442B (zh) 2017-12-12 2017-12-12 一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法

Country Status (1)

Country Link
CN (1) CN108120442B (zh)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109828600B (zh) * 2019-01-09 2020-07-24 北京理工大学 时间最优快速三维避障路径规划方法
CN109976154A (zh) * 2019-03-04 2019-07-05 北京理工大学 一种基于混沌多项式和序列凸优化的飞行器轨迹优化方法
CN111752295B (zh) * 2019-08-27 2021-09-10 广州极飞科技股份有限公司 无人机飞行轨迹规划方法及相关装置
CN110562493B (zh) * 2019-09-06 2021-03-30 北京理工大学 基于矢量轨迹的火星动力下降轨迹规划方法
CN110716581B (zh) * 2019-10-11 2022-12-27 南京理工大学 基于施瓦尔兹不等式的飞行器避障方法
CN110989626B (zh) * 2019-12-27 2021-07-27 四川大学 一种基于控制参数化的无人机路径规划方法
CN111399439B (zh) * 2020-03-02 2021-07-06 之江实验室 一种基于二阶锥规划的移动作业机器人的轨迹生成方法
CN111479226B (zh) * 2020-05-22 2022-03-11 南京邮电大学 无人机通信系统及基于用户轨迹的无人机通信优化方法
CN111948940B (zh) * 2020-07-10 2021-05-04 北京航空航天大学 一种基于动态最优控制的倾转旋翼无人机轨迹优化方法
CN113741414A (zh) * 2021-06-08 2021-12-03 北京理工大学 一种基于移动机器人轮廓的安全运动规划方法和装置
CN113566826B (zh) * 2021-07-08 2023-11-07 北京航空航天大学 一种基于二次规划的无人机路径规划方法
CN113777926B (zh) * 2021-09-15 2023-11-10 北京理工大学 小天体附着三维凸轨迹燃耗最优控制方法
CN117055608B (zh) * 2023-10-11 2023-12-29 北京理工大学 一种基于锂电动力特性动态逆的固定翼无人机制导方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104501816A (zh) * 2015-01-08 2015-04-08 中国航空无线电电子研究所 一种多无人飞行器协调避碰导引规划方法
CN105929844A (zh) * 2016-04-26 2016-09-07 哈尔滨工业大学 一种地外天体软着陆多障碍约束环境下避障方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11322031B2 (en) * 2015-12-07 2022-05-03 International Business Machines Corporation Method of optimized path planning for UAVs for the purpose of ground coverage

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104501816A (zh) * 2015-01-08 2015-04-08 中国航空无线电电子研究所 一种多无人飞行器协调避碰导引规划方法
CN105929844A (zh) * 2016-04-26 2016-09-07 哈尔滨工业大学 一种地外天体软着陆多障碍约束环境下避障方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Sequential Convex Programming for Nonlinear Optimal Control Problem in UAV Path Planning;Zhang Zhe,etc;《2017 AMERICAN CONTROL CONFERENCE (ACC)》;20170531;第1966-1971页 *
基于凸优化的无人机三维避障轨迹规划;王祝，等;《第28届中国控制与决策会议论文集（下）》;20161231;第5798-5802页 *
基于罚函数序列凸规划的多无人机轨迹规划;王祝，等;《航空学报》;20161025;第37卷(第10期);第3149-3158页 *

Also Published As

Publication number Publication date
CN108120442A (zh) 2018-06-05

Similar Documents

Publication Publication Date Title
CN108120442B (zh) 一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法
Rubí et al. A survey of path following control strategies for UAVs focused on quadrotors
Gandolfo et al. Stable path-following control for a quadrotor helicopter considering energy consumption
Moore et al. Robust post-stall perching with a simple fixed-wing glider using LQR-Trees
CN110989626B (zh) 一种基于控制参数化的无人机路径规划方法
Kazim et al. Disturbance-rejection-based optimized robust adaptive controllers for UAVs
Alexis et al. Constrained optimal attitude control of a quadrotor helicopter subject to wind-gusts: experimental studies
Zhao et al. Model-free control of quad-rotor vehicle via finite-time convergent extended state observer
Zhai et al. Control and navigation system for a fixed-wing unmanned aerial vehicle
Stanculeanu et al. Quadrotor black-box system identification
Jung et al. A trajectory-tracking controller design using l 1 adaptive control for multi-rotor uavs
Jeong et al. Control System Design for a Ducted‐Fan Unmanned Aerial Vehicle Using Linear Quadratic Tracker
Jatsun et al. Control fligth of a UAV type tricopter with fuzzy logic controller
Ferdaus et al. A generic self-evolving neuro-fuzzy controller based high-performance hexacopter altitude control system
Wang et al. Practical control implementation of tri-tiltRotor flying wing unmanned aerial vehicles based upon active disturbance rejection control
Velagić et al. Design of LQR controller for 3D trajectory tracking of octocopter unmanned aerial vehicle
Nemes et al. Energy Efficient Feasible Autonomous Multi-Rotor Unmanned Aerial Vehicles Trajectories
Hegde et al. Transition flight modeling and robust control of a VTOL unmanned quad tilt-rotor aerial vehicle
Yang et al. A decentralised control strategy for formation flight of unmanned aerial vehicles
Liu et al. Quaternion-based robust attitude control for quadrotors
Tang et al. Realization of the flight control for an indoor UAV quadrotor
Ahsan et al. Optimization based tuning of autopilot gains for a fixed wing UAV
CN115542947A (zh) 一种四旋翼无人机的自主导航方法及无人机装置
Dong et al. Economic model-predictive control for aircraft forced landing: Framework and two-level implementation
Chen et al. UAVs formation flight control based on following of the guidance points

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
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20210212

Termination date: 20211212