CN115421506B - 一种基于模型预测控制的无人机周期轨迹跟踪及避障方法 - Google Patents

Abstract

本发明公开了一种基于模型预测控制的无人机周期轨迹跟踪及避障方法，其基本思想为：给出无人机的运动模型，以及控制过程中存在的控制输入约束和避障约束；为减轻计算负担和避免求解结果的保守性，采用MINVO基对避障约束进行处理；根据所给的周期参考轨迹，考虑该参考轨迹可能存在不可跟踪的情况，引入人工参考变量，生成新的可跟踪的最优周期轨迹，并根据期待的周期参考轨迹、可跟踪的最优周期轨迹以及无人机的预测轨迹，设计相应的代价函数，构造MPC优化问题，并采用合适的非线性求解器(本发明采用的IPOPT求解器)进行求解，从而完成对无人机周期轨迹跟踪控制。

Description

一种基于模型预测控制的无人机周期轨迹跟踪及避障方法

技术领域

本发明涉及无人机控制技术领域，具体涉及一种基于模型预测控制的无人 机周期轨迹跟踪及避障方法。

背景技术

无人机因其可以在非人工参与情况及危险、枯燥的环境中执行某些复杂的 任务而得到了广泛的关注，在很多领域得到了广泛的应用，如环境监测、人员 搜救、航拍等。而轨迹跟踪控制对于无人机的任务执行而言具有重要意义，并 已经有很多种高效的控制方法应用于无人机的轨迹跟踪控制中，如PID控制、 自抗扰技术、模型预测控制(简称MPC)等。

无人机在任务执行的过程中，会受到各种各样的约束，比如扭矩约束、空 间及速度上的约束。而在上述所提的控制方法中，只有MPC具有直接处理状态 约束和控制输入约束的能力，其他方法只能通过调节参数的方法间接处理约束。 因此，本发明采用了MPC作为控制器来实现轨迹跟踪任务。

同时，无人机在执行任务的过程中，要躲避周围环境各种各样的障碍物， 安全性需要得到保障。轨迹规划是一种常用的方法，即根据实时的动态环境和 已知的参考轨迹生成一条安全的轨迹，常用的轨迹规划方法有A*算法，人工势 能场法，RRT算法等。然而，这些方法一般需要较长的时间去搜索一条安全轨 迹，并且会在实时控制问题中造成额外的计算负担。另一种常用的方法是将避障问题转换为由数学关系表达的避障约束，并把避障约束引入MPC优化问题中， 即将轨迹规划问题和跟踪控制问题集成到一个MPC优化问题中。为了获得避障 约束，可以将障碍物的轨迹进行外近似，即求得一个可以包围障碍物在未来一段时间内运动范围的外多面体，常用的外近似方法有贝塞尔曲线法和B样条曲 线法，但它们获得的外多面体都不是最小的，导致MPC优化问题的求解会有一 定的保守性。这里，我们采用MINVO基的方式来求解一个具有最小体积的外多 面体，并将分离平面作为MPC优化问题中的决策变量，使得无人机轨迹与外多 面体分隔开，从而得到避障约束的数学关系式并引入优化问题中。

发明内容

有鉴于此，本发明提供了一种基于模型预测控制的无人机周期轨迹跟踪及 避障方法，能够解决在具有各种障碍物共存的有限空间内无人机的周期轨迹跟 踪控制问题。

为达到上述目的，本发明的技术方案基于模型预测控制的无人机周期轨迹 跟踪及避障方法，包括如下步骤：

步骤1：构建无人机的运动学模型，提出控制过程中存在的控制输入约束和 避障约束。

步骤2：采用MINVO基，得到可以包围障碍物未来一段时间运动范围的外 多面体，并将其作为障碍物集，通过分离平面将障碍物集和无人机预测轨迹分 离开，得到新的避障约束。

步骤3：考虑控制输入约束、避障约束以及周期参考轨迹，根据运动学模型， 设计相应的代价函数，引入人工参考变量，构造MPC优化问题。

步骤4：针对构造的MPC优化问题进行求解，获得无人机的周期轨迹并实 现避障。

进一步地，步骤1具体包括如下步骤：

步骤1.1：构建无人机的运动学模型为

其中，p＝[x,y,z]^T代表无人机在x轴、y轴和z轴的位置信息；v＝[v_x,v_y,v_z]^T代 表代表无人机在x轴、y轴和z轴的速度信息；ξ＝[p^T,v^T]^T＝[x,y,z,v_x,v_y,v_z]^T为系 统的状态量；u＝[u_x,u_y,u_z]^T为系统在x轴、y轴和z轴的控制输入量；A为状态 系数矩阵；B为控制系数矩阵；C为输出矩阵；

步骤1.2：考虑到运动学模型受到重力加速度的影响，系统的控制输入约束 为：

其中，g代表重力加速度，代表控制输入量的上界，是已知的正常数；/>为输出集；/>为三维空间；

步骤1.3：有n个障碍物时，对于障碍物由/>表示障碍 物在时刻t的真实位置，则无人机在t时刻的避障约束表示为：

其中，d代表最小安全距离。为障碍物集合，p(t)为无人机在t时刻的位 置。

进一步地，步骤2具体包括如下步骤：

步骤2.1：取预测时域为T_p，采样周期为δ，并有T_p＝Nδ，N为预测时域内 的采样周期个数；取时间间隔的下标为并且假设障碍物的预 测轨迹是已知的，表达式为

其中q_j(t)为障碍物j的轨迹；q_j(t+τ_i|t)表示障碍物在时刻t，未来τ_i步的预 测位置，表达式为

其中，Q_j(t+τ_i|t)为与q_j(t+τ_i|t)相关的系数矩阵，s代表预测多项式曲线的阶数；

步骤2.2：对于障碍物在时刻t，未来τ_i步的的实际位置满足

其中，conv{·}表示凸包集合，B_j表示障碍物的膨胀外壳，受自身尺寸β_j和预 测轨迹误差α_j的影响，为闵可夫斯基和；

步骤2.3：采用MINVO基求解最优最小体积的外多面体，外多面体的顶点 集合V_j(t+τ_i|t)和障碍物预测轨迹的多项式曲线系数矩阵Q_j(t+τ_i|t)之间的关系为：

V_j(t+τ_i|t)＝Q_j(t+τ_i|t)A^-1(t+τ_i|t) (7)

其中，A(t+τ_i|t)是随着时间范围变化的矩阵，通过Jesus Tordesillas的MINVO理论得到；

步骤2.4：定义一个障碍物集为

根据步骤2.2和步骤2.3，易得

步骤2.5：分别过障碍物集的每一个顶点以及无人机预测位置做相互平行的 平面P和平面o_j，平面的表达式为

其中，为障碍物集O_j(t+τ_i|t)的顶点集合；o_j(t+τ_i|t)为/>的某个顶点；在时刻t，未来τ_i步中，/>为平面P和平面o_j的单位法向量； d_p为平面P的截距；/>为平面o_j的截距；p(t+τ_i|t)为预测位置；

根据(3)的要求，需满足将(10)改写， 得到的新的避障约束为

进一步地，步骤3具体包括以下步骤：

步骤3.1：对于一个给定的周期参考轨迹r(t)，因其自身约束或障碍物在参 考轨迹而导致无法跟踪，引入一条新的可跟踪周期参考轨迹；通过引入人工参 考变量，设计的代价函数为

J(r,ξ,t；u_r,ξ_r,n_rj,u,ξ,n_j)＝J_t+J_f+J_p (12)

其中，(u,ξ,n_j)表示预测轨迹的控制输入序列、状态序列以及分离平面法向 序列，(u_r,ξ_r,n_rj)表示可跟踪周期参考轨迹的控制输入序列、状态序列以及分离平 面法向量序列，代价函数包括三个部分即J_t表示可跟踪周期参考轨迹与无人机 预测轨迹之间的偏差，J_f代表终端函数，J_p代表可跟踪周期轨迹与期待的周期 参考轨迹之间的偏差；代价函数的每个部分表达式为：

其中，T为参考轨迹的周期，满足T_p＜T和T/δ＝N_r，Q、R、P和S都是正 定矩阵；(t+τ|t)为时刻t，未来τ步的预测信息，u_r(t+τ|t)为时刻t，未来τ步 的参考输入量，u(t+τ|t)为时刻t，未来τ步的实际输入量；ξ(t+T_p|t)为时刻t， 未来T_p步的实际状态量；ξ_r(t+T_p|t)为时刻t，未来T_p步的参考状态量；r(t+τ|t)为 已知的期望的参考轨迹；p_r(t+τ|t)为新生成的参考轨迹；

步骤3.2：在时刻t，关于周期轨迹跟踪的MPC优化问题描述为如下优化问 题1：

约束条件如下：

ξ(t|t)＝ξ(t) 约束(14a)

p(t+τ|t)＝Cξ(t+τ|t),τ∈[0,T_p] 约束(14c)

p_r(t+τ|t)＝Cξ_r(t+τ|t),τ∈[0,T] 约束(14h)

ξ_r(t+T_p|t)-ξ(t+T_p|t)∈Ω 约束(14l)

ξ_r(t|t)＝ξ_r(t+T|t) 约束(14m)

其中，ξ(t|t)为t时刻，无人机的预测初始位置；ξ(t)为t时刻无人机的位置 信息，为在时刻t，未来τ_i步的参考法向量，p_r(t+τ_i|t)为对应时刻无人 机的参考位置；ξ_r(t|t)表示新生成参考轨迹的预测初始位置，ξ_r(t+T|t)表示新生 成参考轨迹在时刻t，未来T步后的预测位置；Ω为终端域；/>并有T＝N_rδ＝；d_r是安全距离；

集合是一个包含在/>内部的闭集合，满足

并且有 为参考输入量集合；/>为参考输入量的最大值；

所述优化问题1中的最优解为其中最优序列中/>和/>代表最优可跟踪参考轨迹相关的状态序列、输出序列和控制输入序列，/>代表 最优可跟踪参考轨迹与障碍物之间分离平面的法向量序列，ξ^*，p^*和u^*代表最 优预测轨迹相关的状态序列、输出序列和控制输入序列，/>代表最优可跟踪参 考轨迹与障碍物之间分离平面的法向量序列；

约束(14a)-约束(14c)表示从初始时刻出发的预测轨迹；

约束(14d)-约束(14e)表示与预测轨迹相关的避障约束；

约束(14f)表示与预测轨迹相关的输入控制约束；

约束(14g)-约束(14h)表示与可跟踪轨迹相关的状态方程；

约束(14i)-约束(14j)表示与可跟踪轨迹相关的避障约束；

约束(14k)表示与可跟踪轨迹相关的输入控制约束；

终端约束(14l)-约束(14m)保证系统的可行性和稳定性；

步骤3.3：根据终端约束(14l)，终端控制器设计为：

u_f＝K(ξ-ξ_r)+u_r (17)

其中终端系数矩阵K＝[diag{k₁₁,k₁₂,k₁₃},diag{k₂₁,k₂₂,k₂₃}]，k₁₁,k₁₂,k₁₃,k₂₁,k₂₂,k₂₃均为 终端系数，满足对于任意i＝1,2，j＝1,2,3，满足k_ij＜0和

步骤3.4：终端域Ω为

并且正定矩阵P满足

P(A+BK)+(A+BK)^TP+Q+K^TRK≤0 (19)

进一步地，安全距离d满足如下条件：

d为安全距离，d_r为更保守的安全距离；ε为终端域相关的一个参数，λ(P) 为P的最小特征值。

有益效果：

1：本发明提供了一种基于模型预测控制的无人机周期轨迹跟踪及避障方法， 为了减少计算量，避免控制过程中的保守性，采用了MINVO基来获取可以包围 障碍物未来一段时间内运动范围的具有最小体积的外多面体，并在每个时间步长引入一系列的分离平面作为MPC优化问题的决策变量，作为避障约束引入 MPC优化问题中以避免与障碍物之间的碰撞；

2：本发明提供了一种基于模型预测控制的无人机周期轨迹跟踪及避障方法， 由于障碍物可能运动到周期参考轨迹上，使得参考轨迹变成不可跟踪的轨迹， 本发明引入了人工参考变量，在每个采样时间进行优化，生成一条相对于期望 参考轨迹的可跟踪的最优周期参考轨迹，并对新的参考轨迹进行跟踪；

3.本发明提供了一种基于模型预测控制的无人机周期轨迹跟踪及避障方法， 其中对终端要素进行了分析，终端要素包含了终端控制器、终端代价函数以及 终端约束，这些终端要素保证了本发明算法的可行性和稳定性。

附图说明

图1为本发明提供了一种基于模型预测控制的无人机周期轨迹跟踪及避障 方法流程图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供了一种基于模型预测控制的无人机周期轨迹跟踪及避障算法， 其基本思想为：给出无人机的运动模型，以及控制过程中存在的控制输入约束 和避障约束；为减轻计算负担和避免求解结果的保守性，采用MINVO基对避障 约束进行处理；根据所给的周期参考轨迹，考虑该参考轨迹可能存在不可跟踪的情况，引入人工参考变量，生成新的可跟踪的最优周期轨迹，并根据期待的 周期参考轨迹、可跟踪的最优周期轨迹以及无人机的预测轨迹，设计相应的代 价函数，构造MPC优化问题，并采用合适的非线性求解器(本发明采用的IPOPT 求解器)进行求解，从而完成对无人机周期轨迹跟踪控制。

本发明提供了一种无人机的轨迹跟踪避障控制方法，如图1所示，具体包 括以下步骤：

步骤1：构建无人机的运动学模型，提出控制过程中存在的控制输入约束和 避障约束。具体包括以下步骤：

步骤1.1：无人机的运动学模型为

其中，p＝[x,y,z]^T代表无人机在x轴、y轴和z轴的位置信息，v＝[v_x,v_y,v_z]^T代 表代表无人机在x轴、y轴和z轴的速度信息，ξ＝[p^T,v^T]^T＝[x,y,z,v_x,v_y,v_z]^T为系 统的状态量，u＝[u_x,u_y,u_z]^T为系统在x轴、y轴和z轴的控制输入量。A为状态 系数矩阵；B为控制系数矩阵；C为输出矩阵；

步骤1.2：考虑到运动学模型会受到重力加速度的影响，系统的控制输入约 束可以写成

其中，g代表重力加速度，代表控制输入量的上界，是已知的正常数。/>

为输出集；为三维空间；

步骤1.3：假设有n个障碍物，对于障碍物由/>表示障 碍物在时刻t的真实位置，无人机的避障约束可以表示为

其中，d代表最小安全距离。为障碍物集合，p(t)为无人机在t时刻的 位置；

步骤2：采用MINVO基，得到可以包围障碍物未来一段时间运动范围的外 多面体，并将其作为障碍物集，通过分离平面将障碍物集和无人机预测轨迹分 离开，得到新的避障约束。具体包括以下步骤：

步骤2.1：取预测时域为T_p，采样周期为δ，并有T_p＝Nδ，N为预测时域内 的采样周期个数。取时间间隔的下标为并且假设障碍物的预 测轨迹是已知的，表达式为

其中q_j(t)为障碍物j的轨迹；q_j(t+τ_i|t)表示障碍物在时刻t，未来τ_i

步的预测位置，表达式为

其中，Q_j(t+τ_i|t)为与q_j(t+τ_i|t)相关的系数矩阵，s代表预测多项式曲线的阶数。

步骤2.2：对于障碍物的实际位置满足

其中，conv{·}表示凸包，B_j表示障碍物的膨胀外壳，受自身尺寸β_j和预测轨 迹误差α_j的影响，为闵可夫斯基和。

步骤2.3：采用MINVO基求解最优最小体积的外多面体，外多面体的顶点 集合V_j(t+τ_i|t)和障碍物预测轨迹的多项式曲线系数矩阵Q_j(t+τ_i|t)之间的关系为：

V_j(t+τ_i|t)＝Q_j(t+τ_i|t)A^-1(t+τ_i|t) (7)

其中，A(t+τ_i|t)是随着时间范围变化的矩阵，可以通过Jesus Tordesillas的MINVO理论得到。

步骤2.4：定义一个障碍物集为

根据步骤2.2和步骤2.3，易得

步骤2.5：分别过障碍物集的每一个顶点以及无人机预测位置做相互平行的 平面P和平面o_j，平面的表达式为

其中，为障碍物集O_j(t+τ_i|t)的顶点集合，o_j(t+τ_i|t)为/>的某个顶点。在时刻t，未来τ_i步中，/>为平面P和平面o_j的单位法向量， d_p为平面P的截距，/>为平面o_j的截距，p(t+τ_i|t)为预测位置。

根据(3)的要求，需满足将(10)改写， 得到的新的避障约束为

步骤3：考虑控制输入约束、避障约束以及周期参考轨迹，根据运动学模型， 引入人工参考变量，设计相应的代价函数，构造MPC优化问题。具体包括以下 步骤：

步骤3.1：对于一个给定的周期参考轨迹r(t)，因其自身约束或障碍物在参 考轨迹而可能导致无法跟踪，需要引入一条新的可跟踪周期参考轨迹。通过引 入人工参考变量，设计的代价函数为

其中，(u,ξ,n_j)表示预测轨迹的控制输入序列、状态序列以及分离平面法向 序列，(u_r,ξ_r,n_rj)表示可跟踪周期参考轨迹的控制输入序列、状态序列以及分离平 面法向量序列，代价函数的每个部分表达式为

其中，T为参考轨迹的周期，满足T_p＜T和T/δ＝N_r。J_t表示可跟踪周期参考 轨迹与无人机预测轨迹之间的偏差，J_f代表终端函数，J_p代表可跟踪周期轨迹 与期待的周期参考轨迹之间的偏差，Q、R、P和S都是正定矩阵。这里，(t+τ|t) 为时刻t，未来τ步的预测信息，如u_r(t+τ|t)为时刻t，未来τ步的参考输入量， u(t+τ|t)为时刻t，未来τ步的实际输入量；ξ(t+T_p|t)为时刻t，未来T_p步的实际 状态量；ξ_r(t+T_p|t)为时刻t，未来T_p步的参考状态量；r(t+τ|t)为已知的期望的 参考轨迹；p_r(t+τ|t)为新生成的参考轨迹。

步骤3.2：在时刻t，关于周期轨迹跟踪的MPC优化问题可以描述为

优化问题1

p(t+τ|t)＝Cξ(t+τ|t),τ∈[0,T_p] 约束(14c)

p_r(t+τ|t)＝Cξ_r(t+τ|t),τ∈[0,T] 约束(14h)

ξ_r(t+T_p|t)-ξ(t+T_p|t)∈Ω 约束(14l)

ξ_r(t|t)＝ξ_r(t+T|t) 约束(14m)

其中，ξ(t|t)为t时刻，无人机的预测初始位置；ξ(t)为t时刻无人机的位置 信息；为在时刻t，未来τ_i步的参考法向量，p_r(t+τ_i|t)为对应时刻无人 机的参考位置。ξ_r(t|t)表示新生成参考轨迹的预测初始位置，ξ_r(t+T|t)表示新生 成参考轨迹在时刻t，未来T步后的预测位置；Ω为终端域，在步骤3.4给出介 绍。/>并有T＝N_rδ＝；d_r是一个保守的安全距离，满足

其中d为安全距离，d_r为更保守的安全距离；ε为终端域相关的一个参数， 在步骤3.4会给出参数需要满足的要求；λ(P)为P的最小特征值。

集合是一个包含在/>内部的闭集合，满足

并且有 为参考输入量集合；/>为参考输入量的最大值。优化问 题1中的最优解为/>其中最优序列中/>和/>代表最优可跟 踪参考轨迹相关的状态序列、输出序列和控制输入序列，/>代表最优可跟踪参 考轨迹与障碍物之间分离平面的法向量序列，ξ^*，p^*和u^*代表最优预测轨迹相 关的状态序列、输出序列和控制输入序列，/>代表最优可跟踪参考轨迹与障碍 物之间分离平面的法向量序列。约束(14a-14c)表示从初始时刻出发的预测轨 迹，约束(14d-14e)表示与预测轨迹相关的避障约束，约束(14f)表示与预测 轨迹相关的输入控制约束，约束(14g-14h)表示与可跟踪轨迹相关的状态方程， 约束(14i-14j)表示与可跟踪轨迹相关的避障约束，约束(14k)表示与可跟踪轨迹相关的输入控制约束，终端约束(14l-m)保证了系统的可行性和稳定性。

步骤3.3：根据终端约束(14l)，终端控制器可以设计成

u_f＝K(ξ-ξ_r)+u_r (17)

其中终端系数矩阵K＝[diag{k₁₁,k₁₂,k₁₃},diag{k₂₁,k₂₂,k₂₃}]，满足对于任意i＝1,2， j＝1,2,3，满足k_ij＜0和

步骤3.4：终端域Ω为

并且正定矩阵P满足

P(A+BK)+(A+BK)^TP+Q+K^TRK≤0 (19)

步骤3.5：综上所述，基于模型预测控制的无人机周期轨迹跟踪及避障算法 流程可以描述为

算法1：基于模型预测控制的无人机周期轨迹跟踪及避障算法

离线设计：周期参考轨迹r(t)，安全距离d和d_r，控制输入量最大值和/>周期T，预测时域T_p，采样周期δ，权重矩阵Q,R,S，根据步骤3.4计算P。

1.测量状态量ξ(t)，给定ξ(t|t)＝ξ(t)；

2.通过(8)计算O_j和相应顶点集

3.求解问题1得到最优控制序列u^*(t)；

4.在τ∈[t,t+δ]，将最优控制量u^*(τ|t)应用于实际模型中；

5.更新时间t←t+δ，返回第1步。

步骤4：针对构造的MPC优化问题进行求解，获得无人机的周期轨迹并实 现避障。

在MATLAB上运用YALMIP工具箱和IPOPT求解器进行仿真实验，验证 本发明算法的有效性和可实施性。具体步骤为

仿真参数选取：r(t)＝[2cos(0.4πt),2sin(0.4πt),3]^T，d＝0.5，d_r＝0.3，T＝5s，δ＝0.25s，T_p＝1.5s，Q＝I₆，R＝I₃，S＝100I₆，设置仿真时间为10s，初始位 置为ξ＝[2,1,2,0,0,0]^T，通过步骤3.3-3.4，取∈＝0.1，P为

仿真环境在一个长度只有5m，宽度只有3.5m的密闭空间中，并且限制无 人机飞行最大高度为4.2m，环境内含有4个静态障碍物和一个动态障碍物，仿 真模拟的结果表面无人机可以避开障碍物。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保 护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等， 均应包含在本发明的保护范围之内。

Claims (1)

1.基于模型预测控制的无人机周期轨迹跟踪及避障方法，其特征在于，包括如下步骤：

步骤1：构建无人机的运动学模型，提出控制过程中存在的控制输入约束和避障约束；具体包括如下步骤：

步骤1.1：构建无人机的运动学模型为

其中，p＝[x,y,z]^T代表无人机在x轴、y轴和z轴的位置信息；v＝[v_x,v_y,v_z]^T代表代表无人机在x轴、y轴和z轴的速度信息；ξ＝[p^T,v^T]^T＝[x,y,z,v_x,v_y,v_z]^T为系统的状态量；u＝[u_x,u_y,u_z]^T为系统在x轴、y轴和z轴的控制输入量；A为状态系数矩阵；B为控制系数矩阵；C为输出矩阵；

步骤1.2：考虑到运动学模型受到重力加速度的影响，系统的控制输入约束为：

其中，g代表重力加速度，u代表控制输入量的上界，是已知的正常数；U为输出集；为三维空间；

步骤1.3：有n个障碍物时，对于障碍物j∈O＝{1,2,...,n}，由表示障碍物在时刻t的真实位置，则无人机在t时刻的避障约束表示为：

其中，d代表最小安全距离；O为障碍物集合，p(t)为无人机在t时刻的位置；

步骤2：采用MINVO基，得到可以包围障碍物未来一段时间运动范围的外多面体，并将其作为障碍物集，通过分离平面将障碍物集和无人机预测轨迹分离开，得到新的避障约束；具体包括如下步骤：

步骤2.1：取预测时域为T_p，采样周期为δ，并有T_p＝Nδ，N为预测时域内的采样周期个数；取时间间隔的下标为i∈I＝{0,1,...,N-1}，并且假设障碍物的预测轨迹是已知的，表达式为：

其中q_j(t)为障碍物j的轨迹；q_j(t+τ_i|t)表示障碍物在时刻t，未来τ_i步的预测位置，表达式为：

其中，Q_j(t+τ_i|t)为与q_j(t+τ_i|t)相关的系数矩阵，s代表预测多项式曲线的阶数；

步骤2.2：对于障碍物在时刻t，未来τ_i步的的实际位置满足：

其中，conv{·}表示凸包集合，B_j表示障碍物的膨胀外壳，受自身尺寸β_j和预测轨迹误差α_j的影响，为闵可夫斯基和；

步骤2.3：采用MINVO基求解最优最小体积的外多面体，外多面体的顶点集合V_j(t+τ_i|t)和障碍物预测轨迹的多项式曲线系数矩阵Q_j(t+τ_i|t)之间的关系为：

V_j(t+τ_i|t)＝Q_j(t+τ_i|t)A^-1(t+τ_i|t) (7)

其中，A(t+τ_i|t)是随着时间范围变化的矩阵，通过JesusTordesillas的MINVO理论得到；

步骤2.4：定义一个障碍物集为：

根据步骤2.2和步骤2.3，得到：

步骤2.5：分别过障碍物集的每一个顶点以及无人机预测位置做相互平行的平面P和平面o_j，平面的表达式为：

其中，为障碍物集O_j(t+τ_i|t)的顶点集合；o_j(t+τ_i|t)为/>的某个顶点；在时刻t，未来τ_i步中，/>为平面P和平面o_j的单位法向量；d_p为平面P的截距；/>为平面o_j的截距；p(t+τ_i|t)为预测位置；

根据(3)的要求满足将式(10)改写，得到的新的避障约束为：

步骤3：考虑控制输入约束、避障约束以及周期参考轨迹，根据运动学模型，设计相应的代价函数，引入人工参考变量，构造MPC优化问题；具体包括以下步骤：

步骤3.1：对于一个给定的周期参考轨迹r(t)，因其自身约束或障碍物在参考轨迹而导致无法跟踪，引入一条新的可跟踪周期参考轨迹；通过引入人工参考变量，设计的代价函数为：

J(r,ξ,t；u_r,ξ_r,n_rj,u,ξ,n_j)＝J_t+J_f+J_p (12)

其中，(u,ξ,n_j)表示预测轨迹的控制输入序列、状态序列以及分离平面法向序列，(u_r,ξ_r,n_rj)表示可跟踪周期参考轨迹的控制输入序列、状态序列以及分离平面法向量序列，代价函数包括三个部分即J_t表示可跟踪周期参考轨迹与无人机预测轨迹之间的偏差，J_f代表终端函数，J_p代表可跟踪周期轨迹与期待的周期参考轨迹之间的偏差；代价函数的每个部分表达式为：

其中，T为参考轨迹的周期，满足T_p＜T和T/δ＝N_r，Q、R、P和S都是正定矩阵；(t+τ|t)为时刻t，未来τ步的预测信息，u_r(t+τ|t)为时刻t，未来τ步的参考输入量，u(t+τ|t)为时刻t，未来τ步的实际输入量；ξ(t+T_p|t)为时刻t，未来T_p步的实际状态量；ξ_r(t+T_p|t)为时刻t，未来T_p步的参考状态量；r(t+τ|t)为已知的期望的参考轨迹；p_r(t+τ|t)为新生成的参考轨迹；

步骤3.2：在时刻t，关于周期轨迹跟踪的MPC优化问题描述为优化问题1：

约束条件如下：

ξ(t|t)＝ξ(t) 约束(14a)

p(t+τ|t)＝Cξ(t+τ|t),τ∈[0,T_p] 约束(14c)

u(t+τ|t)∈U,τ∈[0,T_p] 约束(14f)

p_r(t+τ|t)＝Cξ_r(t+τ|t),τ∈[0,T] 约束(14h)

||n_rj(t+τ_i|t)||＝1,i∈I_r 约束(14j)

u_r(t+τ|t)∈U_r 约束(4k)

ξ_r(t+T_p|t)-ξ(t+T_p|t)∈Ω 约束(14l)

ξ_r(t|t)＝ξ_r(t+T|t) 约束(14m)

其中，ξ(t|t)为t时刻，无人机的预测初始位置；ξ(t)为t时刻无人机的位置信息，为在时刻t，未来τ_i步的参考法向量，p_r(t+τ_i|t)为对应时刻无人机的参考位置；ξ_r(t|t)表示新生成参考轨迹的预测初始位置，ξ_r(t+T|t)表示新生成参考轨迹在时刻t，未来T步后的预测位置；Ω为终端域；I_r＝{0,1,...,N_r-1}，并有T＝N_rδ＝；d_r是更保守的安全距离，满足：

d为安全距离，ε为终端域相关的一个参数，λ(P)为P的最小特征值；

集合U_r是一个包含在U内部的闭集合，满足：

并且有U_r为参考输入量集合；/>为参考输入量的最大值；

所述优化问题1中的最优解为其中最优序列中/>和/>代表最优可跟踪参考轨迹相关的状态序列、输出序列和控制输入序列，/>代表最优可跟踪参考轨迹与障碍物之间分离平面的法向量序列，ξ^*，p^*和u^*代表最优预测轨迹相关的状态序列、输出序列和控制输入序列，/>代表最优可跟踪参考轨迹与障碍物之间分离平面的法向量序列；

约束(14a)-约束(14c)表示从初始时刻出发的预测轨迹；

约束(14d)-约束(14e)表示与预测轨迹相关的避障约束；

约束(14f)表示与预测轨迹相关的输入控制约束；

约束(14g)-约束(14h)表示与可跟踪轨迹相关的状态方程；

约束(14i)-约束(14j)表示与可跟踪轨迹相关的避障约束；

约束(14k)表示与可跟踪轨迹相关的输入控制约束；

终端约束(14l)-约束(14m)保证系统的可行性和稳定性；

步骤3.3：根据终端约束(14l)，终端控制器设计为：

u_f＝K(ξ-ξ_r)+u_r (17)

其中终端系数矩阵K＝[diag{k₁₁,k₁₂,k₁₃},diag{k₂₁,k₂₂,k₂₃}]，k₁₁,k₁₂,k₁₃,k₂₁,k₂₂,k₂₃均为终端系数，满足对于任意i＝1,2，j＝1,2,3，满足k_ij＜0和

步骤3.4：终端域Ω为：

并且正定矩阵P满足：

P(A+BK)+(A+BK)^TP+Q+K^TRK≤0 (19)

步骤4：针对构造的MPC优化问题进行求解，获得无人机的周期轨迹并实现避障。