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

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

Info

Publication number
CN112666975B
CN112666975B CN202011506198.2A CN202011506198A CN112666975B CN 112666975 B CN112666975 B CN 112666975B CN 202011506198 A CN202011506198 A CN 202011506198A CN 112666975 B CN112666975 B CN 112666975B
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
control
obstacle
mpc
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.)
Active
Application number
CN202011506198.2A
Other languages
English (en)
Other versions
CN112666975A (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.)
Sun Yat Sen University
Original Assignee
Sun Yat Sen University
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 Sun Yat Sen University filed Critical Sun Yat Sen University
Priority to CN202011506198.2A priority Critical patent/CN112666975B/zh
Publication of CN112666975A publication Critical patent/CN112666975A/zh
Application granted granted Critical
Publication of CN112666975B publication Critical patent/CN112666975B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

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

Description

一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法
技术领域
本发明属于无人机安全轨迹跟踪控制技术领域，更具体地，涉及一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法。
背景技术
随着时代的发展，无人机在电力巡检、农业喷灌等方面有着广泛的应用。在面对不确定障碍物扰动下，需要控制器在保证系统安全前提下，保持高精度的跟踪控制性能。如：在户外无人机农药喷洒任务中，无人机在应该精确跟踪一个上层规划器规划出的轨迹，以完成既定的喷洒作业任务。但不确定的障碍物扰动下，例如：农田上方的飞过的鸟类，可能会造成无人机在轨迹跟踪时的不安全性，使得基于模型的控制器很难有一个高性能的安全跟踪表现。
为达到上述的目标，现有经典技术需要一个高层轨迹规划器规划期望轨迹和一个底层跟踪控制器跟踪此期望轨迹，但是，在障碍物密集场景和存在运动的动态障碍物时，规划器很难实时规划出一条安全的无障碍物轨迹从而难以满足无人机安全跟踪的需求。此外，从控制器角度，对于一个复杂的非线性无人机系统，为控制器设计一个光滑的梯度目标函数满足跟踪和避障需求是很耗时和繁琐的。因此，在存在静态和动态障碍物的轨迹跟踪任务场景中，为控制器设计一个简单非连续的无梯度目标函数，使得能通过当前无人机状态信息和障碍物信息来在线进行轨迹调整，以实现安全性是很有必要的。
发明内容
本发明为克服上述现有技术中的至少一个缺陷，提供一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法，从控制层面基于控制屏障函数来实现在障碍物环境无人机的自适应轨迹调整以保证轨迹跟踪时的安全性。
为解决上述技术问题，本发明采用的技术方案是：一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法，包括以下步骤：
S1.构建无人机非线性仿射系统模型，并设定无人机和障碍物的最小安全距离S；
S2.基于机载传感器获取周围障碍物信息，以及无人机的位置、速度和姿态角信息；
S3.基于无人机的当前位置和目标轨迹以及周围障碍物信息，设置非连续的跟踪和避障损失函数，构造模型预测控制MPC问题，并基于交叉熵算法求解得到控制器输出的动作uMPC以控制无人机，让其跟踪期望目标轨迹和进行障碍物规避；
S4.基于期望的无人机和障碍物的最小安全距离和机载传感器获得的无人机和障碍物距离，通过半正定规划获得最大无障碍椭球域以构造控制屏障函数；基于控制屏障函数构建不等式约束的二次规划问题，对步骤S3得到的预测控制器输出的动作uMPC进行最小修正，投影到安全域动作集合内，得到安全控制输入u*；
S5.将获得的安全控制输入u*应用于当前无人机的与环境的交互中；
S6.重复步骤S2至步骤S5直至交互结束。
在本发明中，通过基于采样的交叉熵算法求解模型预测控制问题，并基于控制控制屏障函数构造带约束安全的优化问题，实现无人机在障碍物场景下的安全轨迹跟踪。
进一步的，所述的步骤S1中，无人机非线性仿射系统建模为：
Figure GDA0003493273940000021
式中，状态量为X＝[x1,x2,x3]T，其中
Figure GDA0003493273940000022
Figure GDA0003493273940000023
分别表示无人机系统的位置、速度和姿态角；控制量u,其中
Figure GDA0003493273940000024
Fu,w1,w2,w3分别为系统的推进力和机体坐标系三个轴的角速度；f(X)为系统矩阵，g(X)为控制矩阵；f(X)为系统矩阵，g(X)为控制矩阵。
进一步的，所述的步骤S3中，模型预测控制算法基于交叉熵方法。交叉熵方法(CEM)是一种基于重要性采样求解如下问题的随机型近似优化方法；基于无人机的当前位置和目标轨迹以及周围障碍物信息，设置非连续的跟踪和避障损失函数，构造模型预测控制MPC问题，并基于交叉熵算法求解得到预测控制器输出的动作uMPC，具体包括：
Figure GDA0003493273940000025
s.t.
Figure GDA0003493273940000031
(系统动力学)
umin(t)<u(t)<umax(t),(控制量约束)
式中，x1(t)为无人机在t时刻的无人机位置状态，x1d(t)为无人机在t时刻的目标位置状态，T为预测时域，
Figure GDA0003493273940000032
为轨迹跟踪项的损失权系数，
Figure GDA0003493273940000033
为控制输入项的损失权系数，C为无人机和障碍物的距离，I(C)为指示函数，当C大于无人机和障碍物的安全距离S时置1，否则为0；求解得到的最优解包含参考控制序列u(t)，
Figure GDA0003493273940000034
选取第一步的控制量uMPC＝u(tk)。
进一步的，在所述的步骤S4中，构建的不等式约束的二次规划问题的公式为：
Figure GDA0003493273940000035
s.t.-Lgh(x)u-Lfh(x)-κh(x)≤η,
umin≤u≤umax,
式中，式中，h(x)为基于无障碍椭球域构造的安全控制屏障函数，u是无人机控制量，umax和umin则对应其上界与下界，对角矩阵H代表每一维动作的权重，η代表松弛变量而Kη是对应的系数，符号L代表李氏导数，
Figure GDA0003493273940000036
和
Figure GDA0003493273940000037
分别表示函数h(x)关于系统矩阵函数和控制矩阵函数的李导数，κ是一个k类函数。
与现有技术相比，有益效果是：本发明提供的一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法，从控制层面解决了非线性动力学模型无人机在动作空间受限的轨迹跟踪任务中，面对静态和动态障碍物存在跟踪不安全的技术问题；通过非连续的目标函数和基于采样的交叉熵算法求解模型预测控制问题得到基准控制量来调节非线性无人机系统的轨迹；基于无人机和障碍物的相对距离构造无障碍椭球域，并通过控制屏障函数在最小化校正预测控制输出的基准控制量调节无人机的速度在将其动作空间约束在安全可行域内，保证无人机在轨迹跟踪过程中的安全性。
附图说明
图1是本发明方法的整体流程示意图。
图2是本发明算法的流程示意图。
具体实施方式
附图仅用于示例性说明，不能理解为对本发明的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本发明的限制。
如图1和图2所示，一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法，包括以下步骤：
步骤1.构建无人机非线性仿射系统模型，和三维空间中的期望目标轨迹xd，并设定无人机和障碍物的最小安全距离S；
无人机非线性仿射系统建模为：
Figure GDA0003493273940000041
式中，状态量为X＝[x1,x2,x3]T，其中
Figure GDA0003493273940000042
Figure GDA0003493273940000043
分别表示无人机系统的位置、速度和姿态角；控制量u,其中
Figure GDA0003493273940000044
Fu,w1,w2,w3分别为系统的推进力和机体坐标系三个轴的角速度；f(X)为系统矩阵，g(X)为控制矩阵；f(X)为系统矩阵，g(X)为控制矩阵。
步骤2.基于无人机的机载传感器获取周围障碍物信息，以及无人机的位置、速度和姿态角信息。
S3.基于无人机的当前位置和目标轨迹xd以及周围障碍物信息，设置非连续的跟踪的损失函数，构造时域为T模型预测控制MPC问题，并基于交叉熵算法求解得到基准控制量序列，选取第一步的控制器输出的动作uMPC，具体形式如下：
Figure GDA0003493273940000045
s.t.
Figure GDA0003493273940000046
(系统动力学)
umin(t)<u(t)<umax(t),(控制量约束)
式中，x1(t)为无人机在t时刻的无人机位置状态，x1d(t)为无人机在t时刻的目标位置状态，T为预测时域，
Figure GDA0003493273940000047
为轨迹跟踪项的损失权系数，
Figure GDA0003493273940000048
为控制输入项的损失权系数，C为无人机和障碍物的距离，I(C)为指示函数，当C大于无人机和障碍物的安全距离S时置1，否则为0；求解得到的最优解包含参考控制序列u(t)，
Figure GDA0003493273940000051
选取第一步的控制量uMPC＝u(tk)。
步骤4.基于期望的最小无人机和障碍物的安全距离和机载传感器获得的无人和障碍物的间距，通过半正定规划获得最大无障碍椭球域得到安全状态空间C＝{x|h(x)≥0}，其中屏障函数h(x)≥0表明无人机位置在安全椭球区域内的。当步骤3的控制器输出的动作uMPC满足此安全约束，则不对其进行修正，u*＝uMPC。否则构造带约束二次规划优化问题在最小校正准则下将uMPC修正至安全域动作集合内，使无人机的状态空间维持在安全域内，得到最佳动作u*；
构建的不等式约束的二次规划问题的具体公式为：
Figure GDA0003493273940000052
s.t.-Lgh(x)u-Lfh(x)-κh(x)≤η,
umin≤u≤umax,
式中，h(x)为基于无障碍椭球域构造的安全控制屏障函数，u是无人机控制量，umax和umin则对应其上界与下界，对角矩阵H代表每一维动作的权重，η代表松弛变量而Kη是对应的系数，符号L代表李氏导数，
Figure GDA0003493273940000053
和
Figure GDA0003493273940000054
分别表示函数h(x)关于系统矩阵函数和控制矩阵函数的李导数，κ是一个k类函数。
步骤5.将获得的安全控制输入u*应用于当前无人机的与环境的交互中；
步骤6.重复步骤S2至步骤S5直至达到最大实验设定时长。
在本实施例中，无人机安全轨迹跟踪任务中，受控无人机能在存在静态和动态障碍物的场景下，能通过本发明提出的算法实现对无人机轨迹的调节以保证安全性，让受控无人机与障碍物的间距保持在安全距离内。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (1)

1.一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法，其特征在于，包括以下步骤：
S1.构建无人机非线性仿射系统模型，并设定无人机和障碍物的最小安全距离S；构建无人机非线性仿射系统模型为：
Figure FDA0003493273930000011
式中，状态量为X＝[x1，x2，x3]T，其中
Figure FDA0003493273930000012
Figure FDA0003493273930000013
分别表示无人机系统的位置、速度和姿态角；控制量u，其中
Figure FDA0003493273930000014
Fu，w1，w2，w3分别为系统的推进力和机体坐标系三个轴的角速度；f(X)为系统矩阵，g(X)为控制矩阵；
S2.基于机载传感器获取周围障碍物信息，以及无人机的位置、速度和姿态角信息；
S3.基于无人机的当前位置和目标轨迹以及周围障碍物信息，设置非连续的跟踪和避障损失函数，构造模型预测控制MPC问题，并基于交叉熵算法求解得到预测控制器输出的动作uMPC以控制无人机，让其跟踪期望目标轨迹和进行障碍物规避；构造模型预测控制MPC问题，并基于交叉熵算法求解得到预测控制器输出的动作uMPC，具体包括：
Figure FDA0003493273930000015
Figure FDA0003493273930000016
(系统动力学)
umin(t)＜u(t)＜umax(t)，(控制量约束)
式中，x1(t)为无人机在t时刻的无人机位置状态，x1d(t)为无人机在t时刻的目标位置状态，T为预测时域，
Figure FDA0003493273930000017
为轨迹跟踪项的损失权系数，
Figure FDA0003493273930000018
为控制输入项的损失权系数，C为无人机和障碍物的距离，I(C)为指示函数，当C大于无人机和障碍物的安全距离S时置1，否则为0；求解得到的最优解包含参考控制序列u(t)，
Figure FDA0003493273930000019
选取第一步的控制量uMPC＝u(tk)；
S4.基于期望的无人机和障碍物的最小安全距离和机载传感器获得的无人机和障碍物距离，通过半正定规划获得最大无障碍椭球域以构造控制屏障函数；基于控制屏障函数构建不等式约束的二次规划问题，对步骤S3得到的预测控制器输出的动作uMPC进行最小修正，投影到安全域动作集合内，得到安全控制输入u*；构建的不等式约束的二次规划问题的公式为：
Figure FDA0003493273930000021
s.t.-Lgh(x)u-Lfh(x)-κh(x)≤η,
umin≤u≤umax,
式中，h(x)为基于无障碍椭球域构造的安全控制屏障函数，u是无人机控制量，umax和umin则对应其上界与下界，对角矩阵H代表每一维动作的权重，η代表松弛变量而Kη是对应的系数，符号L代表李氏导数，
Figure FDA0003493273930000022
和
Figure FDA0003493273930000023
分别表示函数h(x)关于系统矩阵函数和控制矩阵函数的李导数，κ是一个k类函数
S5.将获得的安全控制输入u*应用于当前无人机的与环境的交互中；
S6.重复步骤S2至步骤S5直至交互结束。
CN202011506198.2A 2020-12-18 2020-12-18 一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法 Active CN112666975B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011506198.2A CN112666975B (zh) 2020-12-18 2020-12-18 一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011506198.2A CN112666975B (zh) 2020-12-18 2020-12-18 一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法

Publications (2)

Publication Number Publication Date
CN112666975A CN112666975A (zh) 2021-04-16
CN112666975B true CN112666975B (zh) 2022-03-29

Family

ID=75406894

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011506198.2A Active CN112666975B (zh) 2020-12-18 2020-12-18 一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法

Country Status (1)

Country Link
CN (1) CN112666975B (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115236997B (zh) * 2021-05-27 2023-08-25 上海仙途智能科技有限公司 预测控制方法、装置、设备及计算机可读存储介质
CN113778082B (zh) * 2021-08-23 2023-08-22 北京科技大学 一种基于自触发机制的无人车轨迹跟踪控制方法及系统
CN113759983B (zh) * 2021-10-20 2023-05-30 中山大学 基于微分平坦性的抗扰动无人机避碰方法
CN114355976B (zh) * 2021-12-31 2023-12-05 中山大学 一种基于学习的控制无人机在风扰下完成轨迹跟踪的方法
CN114740882B (zh) * 2022-03-03 2024-08-06 浙江大学湖州研究院 一种无人机保证可视性的弹性目标跟踪的轨迹生成方法
CN114721412B (zh) * 2022-03-16 2023-06-16 北京理工大学 一种基于模型预测控制的无人机轨迹跟踪避障方法
CN114675670B (zh) * 2022-04-02 2022-10-14 广东工业大学 一种基于相对定位的无人机自主穿越框型障碍物方法
CN116880213B (zh) * 2023-08-16 2024-02-13 北京航空航天大学 无人机抗干扰安全控制方法及相关产品
CN117666354A (zh) * 2023-12-08 2024-03-08 国网江苏省电力有限公司泰州供电分公司 一种巡检无人机运动规划方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106406354A (zh) * 2016-11-29 2017-02-15 中山大学 一种基于三维动态避障的分布式飞行器编队实现方法
CN109885084A (zh) * 2019-03-08 2019-06-14 南开大学 一种基于单目视觉和模糊控制的多旋翼无人机自主降落方法
CN110632941A (zh) * 2019-09-25 2019-12-31 北京理工大学 一种复杂环境下无人机目标跟踪的轨迹生成方法
CN110908399A (zh) * 2019-12-02 2020-03-24 广东工业大学 一种基于轻量型神经网络的无人机自主避障方法及系统
CN111413966A (zh) * 2020-03-12 2020-07-14 天津大学 一种递进式模型预测无人驾驶规划跟踪协同控制方法
CN112034887A (zh) * 2020-09-10 2020-12-04 南京大学 无人机躲避柱状障碍物到达目标点的最优路径训练方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106406354A (zh) * 2016-11-29 2017-02-15 中山大学 一种基于三维动态避障的分布式飞行器编队实现方法
CN109885084A (zh) * 2019-03-08 2019-06-14 南开大学 一种基于单目视觉和模糊控制的多旋翼无人机自主降落方法
CN110632941A (zh) * 2019-09-25 2019-12-31 北京理工大学 一种复杂环境下无人机目标跟踪的轨迹生成方法
CN110908399A (zh) * 2019-12-02 2020-03-24 广东工业大学 一种基于轻量型神经网络的无人机自主避障方法及系统
CN111413966A (zh) * 2020-03-12 2020-07-14 天津大学 一种递进式模型预测无人驾驶规划跟踪协同控制方法
CN112034887A (zh) * 2020-09-10 2020-12-04 南京大学 无人机躲避柱状障碍物到达目标点的最优路径训练方法

Also Published As

Publication number Publication date
CN112666975A (zh) 2021-04-16

Similar Documents

Publication Publication Date Title
CN112666975B (zh) 一种基于预测控制和屏障函数的无人机安全轨迹跟踪方法
González-Sieira et al. Autonomous navigation for UAVs managing motion and sensing uncertainty
CN109625333A (zh) 一种基于深度增强学习的空间非合作目标捕获方法
Bipin et al. Autonomous navigation of generic monocular quadcopter in natural environment
CN112947572B (zh) 一种基于地形跟随的四旋翼飞行器自适应运动规划方法
Stevšić et al. Sample efficient learning of path following and obstacle avoidance behavior for quadrotors
Wu et al. External forces resilient safe motion planning for quadrotor
Diaz et al. On modeling and position tracking control of the generalized differential driven wheeled mobile robot
CN109760047B (zh) 一种基于视觉传感器的舞台机器人预测控制方法
El Houm et al. Optimal new sliding mode controller combined with modified supertwisting algorithm for a perturbed quadrotor UAV
Khosravian et al. Design of an intelligent controller for station keeping, attitude control, and path tracking of a quadrotor using recursive neural networks
Barzegar et al. Deep reinforcement learning-based adaptive controller for trajectory tracking and altitude control of an aerial robot
Ansarian et al. Multi-objective optimal design of a fuzzy adaptive robust fractional-order PID controller for a nonlinear unmanned flying system
Ghommam et al. Distance‐Based Formation Control for Quadrotors with Collision Avoidance via Lyapunov Barrier Functions
Li et al. A warm-started trajectory planner for fixed-wing unmanned aerial vehicle formation
Feng et al. Uav formation and obstacle avoidance based on improved apf
You et al. A new trajectory tracking control method for fully electrically driven quadruped robot
Olaz et al. Quadcopter neural controller for take-off and landing in windy environments
CN109857146B (zh) 一种基于前馈和权值分配的分层无人机跟踪控制方法
Hou et al. Design and Implementation of a Model Predictive Formation Tracking Control System for Underwater Multiple Small Spherical Robots
Notarstefano et al. Trajectory manifold exploration for the PVTOL aircraft
Touil et al. Image-based visual servoing control of a quadcopter air vehicle
CN114355976A (zh) 一种基于学习的控制无人机在风扰下完成轨迹跟踪的方法
Verma et al. A novel trajectory tracking methodology using structured adaptive model inversion for uninhabited aerial vehicles
Wang Sliding Mode Control Design with Disturbance Observer for a Quadrotor

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