CN112666975A - 一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法 - Google Patents

Abstract

本发明属于无人机安全控制技术领域，更具体地，涉及一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法。所述方法通过交叉熵方法来求解模型预测控制问题，基于无人机和障碍物的距离的信息来设计稀疏的损失函数实现轨迹跟踪和避障。基于控制屏障函数构造安全约束以最小介入方式来修正无人机控制量以保证安全性。本发明能够根据当前的目标轨迹以及和障碍物的距离来自适应调整无人机轨迹，有利于保证轨迹跟踪的安全性。

Description

一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法

技术领域

本发明属于无人机安全轨迹跟踪控制技术领域，更具体地，涉及一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法。

背景技术

随着时代的发展，无人机在电力巡检、农业喷灌等方面有着广泛的应用。在面对不确定障碍物扰动下，需要控制器在保证系统安全前提下，保持高精度的跟踪控制性能。如：在户外无人机农药喷洒任务中，无人机在应该精确跟踪一个上层规划器规划出的轨迹，以完成既定的喷洒作业任务。但不确定的障碍物扰动下，例如：农田上方的飞过的鸟类，可能会造成无人机在轨迹跟踪时的不安全性，使得基于模型的控制器很难有一个高性能的安全跟踪表现。

为达到上述的目标，现有经典技术需要一个高层轨迹规划器规划期望轨迹和一个底层跟踪控制器跟踪此期望轨迹，但是，在障碍物密集场景和存在运动的动态障碍物时，规划器很难实时规划出一条安全的无障碍物轨迹从而难以满足无人机安全跟踪的需求。此外，从控制器角度，对于一个复杂的非线性无人机系统，为控制器设计一个光滑的梯度目标函数满足跟踪和避障需求是很耗时和繁琐的。因此，在存在静态和动态障碍物的轨迹跟踪任务场景中，为控制器设计一个简单非连续的无梯度目标函数，使得能通过当前无人机状态信息和障碍物信息来在线进行轨迹调整，以实现安全性是很有必要的。

发明内容

本发明为克服上述现有技术中的至少一个缺陷，提供一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法，从控制层面基于控制屏障函数来实现在障碍物环境无人机的自适应轨迹调整以保证轨迹跟踪时的安全性。

为解决上述技术问题，本发明采用的技术方案是：一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法，包括以下步骤：

S1.构建无人机非线性仿射系统模型，并设定无人机和障碍物的最小安全距离S；

S2.基于机载传感器获取周围障碍物信息，以及无人机的位置、速度和姿态角信息；

S3.基于无人机的当前位置和目标轨迹以及周围障碍物信息，设置非连续的跟踪和避障损失函数，构造模型预测控制MPC问题，并基于交叉熵算法求解得到控制量u_MPC以控制无人机，让其跟踪期望目标轨迹和进行障碍物规避；

S4.基于期望的无人机和障碍物的最小安全距离和机载传感器获得的无人机和障碍物距离，通过半正定规划获得最大无障碍椭球域以构造控制屏障函数；基于控制屏障函数构建不等式约束的二次规划问题，对步骤S3得到的预测控制器输出的动作u_MPC进行最小修正，投影到安全域动作集合内，得到安全控制输入u*；

S5.将获得的控制输入u*应用于当前无人机的与环境的交互中；

S6.重复步骤S2至步骤S5直至交互结束。

在本发明中，通过基于采样的交叉熵算法求解模型预测控制问题，并基于控制控制屏障函数构造带约束安全的优化问题，实现无人机在障碍物场景下的安全轨迹跟踪。

进一步的，所述的步骤S1中，无人机非线性仿射系统建模为：

X＝f(X)+g(X)u

式中，状态量为X＝[x₁,x₂,x₃]^T，其中

分别表示无人机系统的位置、速度和姿态角；控制量为

为系统的推进力和机体坐标系三个轴的角速度；f(X)为系统矩阵，g(X)为控制矩阵。

进一步的，所述的步骤S3中，模型预测控制算法基于交叉熵方法。交叉熵方法(CEM)是一种基于重要性采样求解如下问题的随机型近似优化方法；基于交叉熵算法求解预测控制MPC问题具体包括：

s.t.X＝f(X)+g(X)u，(系统动力学)

u_min(t)<u(t)<u_max(t),控制量约束)

式中，T为预测时域，

为半正定矩阵，

为正定矩阵，C为无人机和障碍物的距离，I(C)为指示函数，当C大于无人机和障碍物的安全距离S时置1，否则为0；求解得到的最优解包含参考控制序列u(t)，

选取第一步的控制量u_MPC＝u(t_k)。

进一步的，在所述的步骤S4中，构建的不等式约束的二次规划问题的公式为：

s.t.-L_gh(x)u-L_fh(x)-κh(x)≤η,

u_min≤u≤u_max,

式中，式中，h(x)为基于无障碍椭球域构造的安全控制屏障函数，u是无人机控制量，u_max和u_min则对应其上界与下界，对角矩阵H代表每一维动作的权重，η代表松弛变量而K_η是对应的系数，符号L代表李氏导数，κ是一个k类函数。

与现有技术相比，有益效果是：本发明提供的一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法，从控制层面解决了非线性动力学模型无人机在动作空间受限的轨迹跟踪任务中，面对静态和动态障碍物存在跟踪不安全的技术问题；通过非连续的目标函数和基于采样的交叉熵算法求解模型预测控制问题得到基准控制量来调节非线性无人机系统的轨迹；基于无人机和障碍物的相对距离构造无障碍椭球域，并通过控制屏障函数在最小化校正预测控制输出的基准控制量调节无人机的速度在将其动作空间约束在安全可行域内，保证无人机在轨迹跟踪过程中的安全性。

附图说明

图1是本发明方法的整体流程示意图。

图2是本发明算法的流程示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本发明的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本发明的限制。

如图1和图2所示，一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法，包括以下步骤：

步骤1.构建无人机非线性仿射系统模型，和三维空间中的期望目标轨迹x_d，并设定无人机和障碍物的最小安全距离S；

无人机非线性仿射系统建模为：

X＝f(X)+g(X)u

式中，状态量为X＝[x₁，x₂，x₃]^T，其中

分别表示无人机系统的位置、速度和姿态角；控制量为

为系统的推进力和机体坐标系三个轴的角速度；f(X)为系统矩阵，g(X)为控制矩阵。

步骤2.基于无人机的机载传感器获取周围障碍物信息，以及无人机的位置、速度和姿态角信息。

S3.基于无人机的当前位置和目标轨迹x_d以及周围障碍物信息，设置非连续的跟踪的损失函数，构造时域为T模型预测控制MPC问题，并基于交叉熵算法求解得到基准控制量序列，选取第一步的控制量u_MPC，具体形式如下：

s.t.X＝f(X)+g(X)u，(系统动力学)

u_min(t)<u(t)<u_max(t),控制量约束)

式中，T为预测时域,

为半正定矩阵，表示跟踪误差的权重系数；

为正定矩阵，表示控制量的权重；C为无人机和障碍物的距离，I(C)为指示函数，当C大于无人机和障碍物的安全距离S时置1,否则为0。上述优化问题求解得到的最优解包含参考轨迹参考控制序列u(t),

我们选取第一步的控制量u_MPC＝u(t_k)。

步骤4.基于期望的最小无人机和障碍物的安全距离和机载传感器获得的无人和障碍物的间距，通过半正定规划获得最大无障碍椭球域得到安全状态空间C＝{x₁|h(x₁)≥0}，其中屏障函数h(x₁)≥0表明无人机位置在安全椭球区域内的。当步骤3的控制量u_MPC满足此安全约束，则不对其进行修正，u*＝u_MPC。否则构造带约束二次规划优化问题在最小校正准则下将u_MPC修正至安全域动作集合内，使无人机的状态空间维持在安全域内，得到最佳动作u*；

构建的不等式约束的二次规划问题的具体公式为：

s.t.-L_gh(x)u-L_fh(x)-κh(x)≤η,

u_min≤u≤u_max,

式中，h(x)为基于无障碍椭球域构造的安全控制屏障函数，u是无人机控制量，u_max和u_min则对应其上界与下界，对角矩阵H代表每一维动作的权重，η代表松弛变量而K_η是对应的系数，符号L代表李氏导数，κ是一个k类函数。

步骤5.将获得的控制输入u*应用于当前无人机的与环境的交互中；

步骤6.重复步骤S2至步骤S5直至达到最大实验设定时长。

在本实施例中，无人机安全轨迹跟踪任务中，受控无人机能在存在静态和动态障碍物的场景下，能通过本发明提出的算法实现对无人机轨迹的调节以保证安全性，让受控无人机与障碍物的间距保持在安全距离内。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (4)

1.一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法，其特征在于，包括以下步骤：

S1.构建无人机非线性仿射系统模型，并设定无人机和障碍物的最小安全距离S；

S2.基于机载传感器获取周围障碍物信息，以及无人机的位置、速度和姿态角信息；

S3.基于无人机的当前位置和目标轨迹以及周围障碍物信息，设置非连续的跟踪和避障损失函数，构造模型预测控制MPC问题，并基于交叉熵算法求解得到控制量u_MPC以控制无人机，让其跟踪期望目标轨迹和进行障碍物规避；

S4.基于期望的无人机和障碍物的最小安全距离和机载传感器获得的无人机和障碍物距离，通过半正定规划获得最大无障碍椭球域以构造控制屏障函数；基于控制屏障函数构建不等式约束的二次规划问题，对步骤S3得到的预测控制器输出的动作u_MPC进行最小修正，投影到安全域动作集合内，得到安全控制输入u*；

S5.将获得的控制输入u*应用于当前无人机的与环境的交互中；

S6.重复步骤S2至步骤S5直至交互结束。

2.根据权利要求1所述的基于预测控制和屏障函数的无人机安全轨迹跟踪方法，其特征在于，所述的步骤S1中，无人机非线性仿射系统建模为：

X＝f(X)+g(X)u

式中，状态量为X＝[x₁，x₂，x₃]^T，其中

分别表示无人机系统的位置、速度和姿态角；控制量为

为系统的推进力和机体坐标系三个轴的角速度；

f(X)为系统矩阵，g(X)为控制矩阵。

3.根据权利要求2所述的基于预测控制和屏障函数的无人机安全轨迹跟踪方法，其特征在于，所述的步骤S3中，基于交叉熵算法求解预测控制MPC问题具体包括：

s.t.X＝f(X)+g(X)u，(系统动力学)

u_min(t)＜u(t)＜u_max(t)，(控制量约束)

式中，T为预测时域，

为半正定矩阵，

为正定矩阵，C为无人机和障碍物的距离，I(C)为指示函数，当C大于无人机和障碍物的安全距离S时置1，否则为0；求解得到的最优解包含参考控制序列u(t)，

选取第一步的控制量u_MPC＝u(t_k)。

4.根据权利要求3所述的基于预测控制和屏障函数的无人机安全轨迹跟踪方法，其特征在于，在所述的步骤S4中，构建的不等式约束的二次规划问题的公式为：

s.t.-L_gh(x)u-L_fh(x)-κh(x)≤η，

u_min≤u≤u_max，

式中，h(x)为基于无障碍椭球域构造的安全控制屏障函数，u是无人机控制量，u_max和u_min则对应其上界与下界，对角矩阵H代表每一维动作的权重，η代表松弛变量而K_η是对应的系数，符号L代表李氏导数，κ是一个k类函数。

Images