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

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

【技术实现步骤摘要】
一种基于模型预测控制的无人机周期轨迹跟踪及避障方法


[0001]本专利技术涉及无人机控制
，具体涉及一种基于模型预测控制的无人 机周期轨迹跟踪及避障方法。

技术介绍

[0002]无人机因其可以在非人工参与情况及危险、枯燥的环境中执行某些复杂的 任务而得到了广泛的关注，在很多领域得到了广泛的应用，如环境监测、人员 搜救、航拍等。而轨迹跟踪控制对于无人机的任务执行而言具有重要意义，并 已经有很多种高效的控制方法应用于无人机的轨迹跟踪控制中，如PID控制、 自抗扰技术、模型预测控制(简称MPC)等。
[0003]无人机在任务执行的过程中，会受到各种各样的约束，比如扭矩约束、空 间及速度上的约束。而在上述所提的控制方法中，只有MPC具有直接处理状态 约束和控制输入约束的能力，其他方法只能通过调节参数的方法间接处理约束。 因此，本专利技术采用了MPC作为控制器来实现轨迹跟踪任务。
[0004]同时，无人机在执行任务的过程中，要躲避周围环境各种各样的障碍物， 安全性需要得到保障。轨迹规划是一种常用的方法，即根据实时的动态环境和 已知的参考轨迹生成一条安全的轨迹，常用的轨迹规划方法有A*算法，人工势 能场法，RRT算法等。然而，这些方法一般需要较长的时间去搜索一条安全轨 迹，并且会在实时控制问题中造成额外的计算负担。另一种常用的方法是将避 障问题转换为由数学关系表达的避障约束，并把避障约束引入MPC优化问题中， 即将轨迹规划问题和跟踪控制问题集成到一个MPC优化问题中。为了获得避障 约束，可以将障碍物的轨迹进行外近似，即求得一个可以包围障碍物在未来一 段时间内运动范围的外多面体，常用的外近似方法有贝塞尔曲线法和B样条曲 线法，但它们获得的外多面体都不是最小的，导致MPC优化问题的求解会有一 定的保守性。这里，我们采用MINVO基的方式来求解一个具有最小体积的外多 面体，并将分离平面作为MPC优化问题中的决策变量，使得无人机轨迹与外多 面体分隔开，从而得到避障约束的数学关系式并引入优化问题中。

技术实现思路

[0005]有鉴于此，本专利技术提供了一种基于模型预测控制的无人机周期轨迹跟踪及 避障方法，能够解决在具有各种障碍物共存的有限空间内无人机的周期轨迹跟 踪控制问题。
[0006]为达到上述目的，本专利技术的技术方案基于模型预测控制的无人机周期轨迹 跟踪及避障方法，包括如下步骤：
[0007]步骤1：构建无人机的运动学模型，提出控制过程中存在的控制输入约束和 避障约束。
[0008]步骤2：采用MINVO基，得到可以包围障碍物未来一段时间运动范围的外 多面体，并将其作为障碍物集，通过分离平面将障碍物集和无人机预测轨迹分 离开，得到新的避障约束。
[0009]步骤3：考虑控制输入约束、避障约束以及周期参考轨迹，根据运动学模型， 设计相应的代价函数，引入人工参考变量，构造MPC优化问题。
[0010]步骤4：针对构造的MPC优化问题进行求解，获得无人机的周期轨迹并实 现避障。
[0011]进一步地，步骤1具体包括如下步骤：
[0012]步骤1.1：构建无人机的运动学模型为
[0013][0014]其中，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为输出矩阵；
[0015]步骤1.2：考虑到运动学模型受到重力加速度的影响，系统的控制输入约束 为：
[0016][0017]其中，g代表重力加速度，代表控制输入量的上界，是已知的正常数；为输出集；为三维空间；
[0018]步骤1.3：有n个障碍物时，对于障碍物由表示障碍 物在时刻t的真实位置，则无人机在t时刻的避障约束表示为：
[0019][0020]其中，d代表最小安全距离。为障碍物集合，p(t)为无人机在t时刻的位 置。
[0021]进一步地，步骤2具体包括如下步骤：
[0022]步骤2.1：取预测时域为T
p
，采样周期为δ，并有T
p
＝Nδ，N为预测时域内 的采样周期个数；取时间间隔的下标为并且假设障碍物的预 测轨迹是已知的，表达式为
[0023][0024]其中q
j
(t)为障碍物j的轨迹；q
j
(t+τ
i
|t)表示障碍物在时刻t，未来τ
i
步的预 测位置，表达式为
[0025][0026]其中，Q
j
(t+τ
i
|t)为与q
j
(t+τ
i
|t)相关的系数矩阵，s代表预测多项式曲线的阶 数；
[0027]步骤2.2：对于障碍物在时刻t，未来τ
i
步的的实际位置满足
[0028][0029]其中，conv{
·
}表示凸包集合，B
j
表示障碍物的膨胀外壳，受自身尺寸β
j
和预 测轨迹误差α
j
的影响，为闵可夫斯基和；
[0030]步骤2.3：采用MINVO基求解最优最小体积的外多面体，外多面体的顶点 集合V
j
(t+τ
i
|t)和障碍物预测轨迹的多项式曲线系数矩阵Q
j
(t+τ
i
|t)之间的关系为：
[0031]V
j
(t+τ
i
|t)＝Q
j
(t+τ
i
|t)A
‑1(t+τ
i
|t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0032]其中，A(t+τ
i
|t)是随着时间范围变化的矩阵，通过Jesus Tordesillas的MINVO 理论得到；
[0033]步骤2.4：定义一个障碍物集为
[0034][0035]根据步骤2.2和步骤2.3，易得
[0036][0037]步骤2.5：分别过障碍物集的每一个顶点以及无人机预测位置做相互平行的 平面P和平面o
j
，平面的表达式为
[0038][0039]其中，为障碍物集O
j
(t+τ
i
|t)的顶点集合；o
j
(t+τ
i
|t)为的某个顶点；在时刻t，未本文档来自技高网...

【技术保护点】

【技术特征摘要】
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

【专利技术属性】
技术研发人员：戴荔，霍达，王沛展，薛若宸，范子煊，夏元清，孙中奇，崔冰，高寒，
申请(专利权)人：北京理工大学，
类型：发明
国别省市：