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

技术特征：
1.基于模型预测控制的无人机周期轨迹跟踪及避障方法，其特征在于，包括如下步骤：步骤1：构建无人机的运动学模型，提出控制过程中存在的控制输入约束和避障约束；步骤2：采用minvo基，得到可以包围障碍物未来一段时间运动范围的外多面体，并将其作为障碍物集，通过分离平面将障碍物集和无人机预测轨迹分离开，得到新的避障约束；步骤3：考虑控制输入约束、避障约束以及周期参考轨迹，根据运动学模型，设计相应的代价函数，引入人工参考变量，构造mpc优化问题；步骤4：针对构造的mpc优化问题进行求解，获得无人机的周期轨迹并实现避障。2.如权利要求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的真实位置，则无人机在t时刻的避障约束表示为：其中，d代表最小安全距离。为障碍物集合，p(t)为无人机在t时刻的位置。3.如权利要求2所述的方法，其特征在于，所述步骤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)改写，得到的新的避障约束为
4.如权利要求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)约束(14c)约束(14c)约束(14c)p
r
(t τ|t)＝cξ
r
(t τ|t),τ∈[0,t]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
约束(14h)约束(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
11
,k
12
,k
13
},diag{k
21
,k
22
,k
23
}]，k
11
,k
12
,k
13
,k
21
,k
22
,k
23
均为终端系数，满足对于任意i＝1,2，j＝1,2,3，满足k
ij
＜0和步骤3.4：终端域ω为并且正定矩阵p满足p(a bk) (a bk)
t
p q k
t
rk≤0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(19)。5.如权利要求4所述的基于模型预测控制的无人机周期轨迹跟踪及避障方法，其特征在于，所述安全距离d满足如下条件：
d为安全距离，d
r
为更保守的安全距离；ε为终端域相关的一个参数，λ(p)为p的最小特征值。

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


技术研发人员：戴荔 霍达 王沛展 薛若宸 范子煊 夏元清 孙中奇 崔冰 高寒
受保护的技术使用者：北京理工大学
技术研发日：2022.03.28
技术公布日：2022/12/1