CN115576209B - 一种基于极值搜索的无人机位置跟踪控制方法 - Google Patents

一种基于极值搜索的无人机位置跟踪控制方法 Download PDF

Info

Publication number
CN115576209B
CN115576209B CN202211568382.9A CN202211568382A CN115576209B CN 115576209 B CN115576209 B CN 115576209B CN 202211568382 A CN202211568382 A CN 202211568382A CN 115576209 B CN115576209 B CN 115576209B
Authority
CN
China
Prior art keywords
aerial vehicle
unmanned aerial
model
ude
representing
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
CN202211568382.9A
Other languages
English (en)
Other versions
CN115576209A (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.)
Nanjing University of Information Science and Technology
Original Assignee
Nanjing University of Information Science and Technology
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 Nanjing University of Information Science and Technology filed Critical Nanjing University of Information Science and Technology
Priority to CN202211568382.9A priority Critical patent/CN115576209B/zh
Publication of CN115576209A publication Critical patent/CN115576209A/zh
Application granted granted Critical
Publication of CN115576209B publication Critical patent/CN115576209B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明公开了一种基于极值搜索的无人机位置跟踪控制方法包括：基于无人机机体运动规律构建无人机系统数学模型；基于不确定性和扰动估计器UDE的设计原理和无人机系统数学模型建立非线性无人机系统模型，基于非线性无人机系统模型设计UDE；利用极值搜索算法实时优化UDE控制器参数。本发明通过设计UDE来解决无人机系统受不确定性和未知扰动的影响；通过在UDE控制器设计的过程中增加滤波器，实现对非线性无人机系统总扰动的准确估计与补偿；通过利用极值搜索算法对滤波器参数进行实时优化来增强无人机轨迹跟踪器的抗干扰能力，减小不确定性和扰动带来的误差,保证无人机稳定准确实现位置跟踪控制。

Description

一种基于极值搜索的无人机位置跟踪控制方法
技术领域
本发明涉及无人机跟踪控制技术领域，具体为一种基于极值搜索的无人机位置跟踪控制方法。
背景技术
四旋翼无人机是一种欠驱动强耦合非线性系统，其飞行控制极易受模型不确定性和风力扰动影响，出现较大偏差。
鉴于以上所述问题，可采用不确定性和扰动估计器（UDE），来解决模型不确定性和风力扰动的影响，但UDE并不能解决传感器扰动带来的影响。因此引入极值搜索方法（ES）对UDE进行参数整定，设计抗干扰能力强的无人机轨迹跟踪控制器，以减小不确定性和扰动带来的误差，保证无人机稳定准确实现位置跟踪控制。
发明内容
本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。
鉴于上述存在的问题，提出了本发明。
因此，本发明解决的技术问题是：四旋翼无人机位置跟踪控制中因模型不确定性、风力扰动和传感器误差影响，出现较大偏差的问题。
为解决上述技术问题，本发明提供如下技术方案：一种基于极值搜索的无人机位置跟踪控制方法，包括：
基于无人机机体运动规律构建无人机系统数学模型；
基于不确定性和扰动估计器UDE的基本设计原理和所述无人机系统数学模型建立非线性无人机系统模型，并基于所述非线性无人机系统模型设计UDE；
利用极值搜索算法对所述UDE的控制器参数进行实时优化。
作为本发明所述的一种基于极值搜索的无人机位置跟踪控制方法的一种优选方案，其中：所述构建无人机系统数学模型，包括：构建无人机运动学模型和无人机动力学模型。
作为本发明所述的一种基于极值搜索的无人机位置跟踪控制方法的一种优选方案，其中：所述构建无人机运动学模型，包括：
将无人机关于位置的方程表示为：
Figure SMS_1
将姿态角的变化率与机体的旋转角速度之间的关系方程表示为：
Figure SMS_2
将无人机关于位置的方程和姿态角的变化率域机体旋转角之间的关系方程相结合，获取无人机关于欧拉角的运动学模型，表示为：
Figure SMS_3
其中，
Figure SMS_4
表示无人机在地球坐标系的坐标位置；
Figure SMS_5
表示无人机在地球坐标系的速 度；
Figure SMS_6
表示姿态角变化率；
Figure SMS_7
表示姿态角变化率和旋转角速度之间的关系系数；
Figure SMS_8
表示旋 转角速度。
作为本发明所述的一种基于极值搜索的无人机位置跟踪控制方法的一种优选方案，其中：所述构建无人机动力学模型，包括：使用牛顿-欧拉方程建立无人机的动力学模型，具体为：
构建无人机位置动力学模型方程，表示为:
Figure SMS_9
在机体坐标系内构建有关四旋翼无人机姿态的动力学关系，表示为：
Figure SMS_10
联立位置动力学模型方程和姿态动力学模型方程以获取无人机动力学模型方程，表示为：
Figure SMS_11
其中，
Figure SMS_19
表示无人机在地球坐标系的速度变化率即加速度，
Figure SMS_17
表示无人机的质 量，
Figure SMS_27
表示重力加速度，
Figure SMS_18
是沿着地球坐标系中垂直于地平面向下的单位向量，
Figure SMS_22
表示旋 转矩阵，
Figure SMS_13
表示旋翼的合力，
Figure SMS_25
表示无人机转动惯量，
Figure SMS_14
为四旋翼无人机在机体坐标系的角 速度;
Figure SMS_24
表示来自于陀螺转轴的角速度而产生的陀螺力矩;
Figure SMS_12
包括旋翼绕
Figure SMS_20
轴、
Figure SMS_16
轴和
Figure SMS_23
轴分别产生的滚转力矩
Figure SMS_21
、俯仰力矩
Figure SMS_26
和偏航力矩
Figure SMS_15
。
作为本发明所述的一种基于极值搜索的无人机位置跟踪控制方法的一种优选方案，其中：所述无人机系统数学模型，包括：将无人机数学模型进行解耦获得水平位置模型、高度位置模型和姿态模型三个线性模型；
水平位置模型表示为：
Figure SMS_28
其中，
Figure SMS_29
表示水平位置变化率即水平速度，
Figure SMS_30
表示水平方向速度，
Figure SMS_31
表示水平方 向速度的变化率即水平加速度，
Figure SMS_32
表示重力加速度，
Figure SMS_33
表示转换矩阵，
Figure SMS_34
表示滚转角和俯 仰角；
高度位置模型表示为：
Figure SMS_35
其中，
Figure SMS_36
表示竖直位置变化率即竖直速度，
Figure SMS_37
表示竖直方向速度，
Figure SMS_38
表示竖直方向 速度的变化率即水平加速度，
Figure SMS_39
表示重力加速度，
Figure SMS_40
表示螺旋桨总拉力大小，
Figure SMS_41
表示无人机 的质量；
姿态模型表示为：
Figure SMS_42
其中，
Figure SMS_43
表示姿态角变化率，
Figure SMS_44
表示在机体坐标系的角速度，
Figure SMS_45
表示无人机转动惯 量，
Figure SMS_46
表示在机体坐标系的角速度变化率，
Figure SMS_47
表示螺旋桨在机体轴上产生的力矩。
作为本发明所述的一种基于极值搜索的无人机位置跟踪控制方法的一种优选方案，其中：所述非线性无人机系统模型，表示为：
Figure SMS_48
作为本发明所述的一种基于极值搜索的无人机位置跟踪控制方法的一种优选方 案，其中：所述设计UDE，包括：基于所述非线性无人机系统模型获取系统的输入向量即控制 律
Figure SMS_49
，表示为：
Figure SMS_50
将控制律
Figure SMS_51
用拉普拉斯变换表示在
Figure SMS_52
域上, 表达式为:
Figure SMS_53
作为本发明所述的一种基于极值搜索的无人机位置跟踪控制方法的一种优选方 案，其中：所述设计UDE，还包括：在轨迹跟踪器中加入具有单位稳态增益和足够带宽的低通 滤波器
Figure SMS_54
，此时UDE可表示成：
Figure SMS_55
作为本发明所述的一种基于极值搜索的无人机位置跟踪控制方法的一种优选方案，其中：所述设计UDE，还包括：设计基于UDE的控制方法，表示为:
Figure SMS_56
作为本发明所述的一种基于极值搜索的无人机位置跟踪控制方法的一种优选方 案，其中：所述对控制器参数进行实时优化，包括：构建代价函数
Figure SMS_57
并通过极值搜索 算法对UDE 控制器的滤波器参数进行实时优化；
代价函数
Figure SMS_58
表示为：
Figure SMS_59
式中,
Figure SMS_60
表示某次计算的开始时刻,
Figure SMS_61
表示此次计算的结束时刻。
本发明的有益效果：本发明通过设计不确定性和扰动估计器UDE来解决无人机系统受不确定性和未知扰动的影响；通过在UDE控制器设计的过程中增加滤波器，实现对非线性无人机系统总扰动的准确估计与补偿；通过利用极值搜索算法对滤波器参数进行实时优化来增强无人机轨迹跟踪器的抗干扰能力，减小不确定性和扰动带来的误差, 保证无人机稳定准确实现位置跟踪控制。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。其中：
图1为本发明一个实施例提供的一种基于极值搜索的无人机位置跟踪控制方法的整体流程图；
图2为本发明一个实施例提供的经极值搜索算法优化后的UDE控制器控制流程图；
图3为本发明一个实施例提供的无人机坐标系和地球坐标系示意图；
图4为本发明一个实施例提供的UDE控制器控制流程图；
图5为本发明一个实施例提供的离散极值搜索算法的原理图;
图6为本发明一个实施例提供的UDE航迹控制方法与UDE结合ES航迹控制方法的对比仿真实验结果图；
图7为本发明一个实施例提供的UDE航迹控制方法与UDE结合ES航迹控制方法的误差对比仿真结果图。
具体实施方式
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本发明的具体实施方式做详细的说明，显然所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的保护的范围。
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。
其次，此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指同一个实施例，也不是单独的或选择性的与其他实施例互相排斥的实施例。
本发明结合示意图进行详细描述，在详述本发明实施例时，为便于说明，表示器件结构的剖面图会不依一般比例作局部放大，而且所述示意图只是示例，其在此不应限制本发明保护的范围。此外，在实际制作中应包含长度、宽度及深度的三维空间尺寸。
同时在本发明的描述中，需要说明的是，术语中的“上、下、内和外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一、第二或第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。
本发明中除非另有明确的规定和限定，术语“安装、相连、连接”应做广义理解，例如：可以是固定连接、可拆卸连接或一体式连接；同样可以是机械连接、电连接或直接连接，也可以通过中间媒介间接相连，也可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
实施例1
参照图1~5，为本发明的一个实施例，提供了一种基于极值搜索的无人机位置跟踪控制方法，包括：
S1：基于无人机机体运动规律构建无人机系统数学模型；
应说明的是，无人机系统数学模型均基于以下假设进行数学模型的构建，假设具体为：无人机的几何中心与重心重合，结构对称分布；无人机机体不考虑弹性形变，看作刚体；质量和转动惯量不变，不考虑电机间动态差异；无人机只受到自身重力以及旋翼拉力作用，其中重力方向为指向地心，拉力方向垂直无人机机体向上。
更进一步的，基于几何学的无人机机体运动构建运动学模型。
应说明的是，所构建的运动学模型只研究物体中的力与位移间的关系，与重量或受力等无关，四旋翼无人机的运动学模型的输入变量与输出变量的关系一一对应，即速度输入对应位置输出，角速度输入对应姿态输出。
具体的，用
Figure SMS_62
来表示无人机在地球坐标系中的坐标位置, 用
Figure SMS_63
来表示无人机在地球坐标系中的速度, 可得出无人机关于位置的 方程，表示为：
Figure SMS_64
（1）
其中，
Figure SMS_67
表示无人机在地球坐标系的坐标位置，
Figure SMS_72
表示无人机的速度，
Figure SMS_77
表示无人 机在地球坐标系中的
Figure SMS_68
轴方向坐标位置；
Figure SMS_70
表示
Figure SMS_75
轴方向坐标位置；
Figure SMS_78
表示
Figure SMS_65
轴方向坐标位 置；
Figure SMS_69
表示
Figure SMS_73
轴方向速度；
Figure SMS_76
表示
Figure SMS_66
轴方向速度；
Figure SMS_71
表示
Figure SMS_74
轴方向速度；
姿态角的变化率与机体的旋转角速度之间的关系方程表示为：
Figure SMS_79
（2）
其中，
Figure SMS_80
表示姿态角变化率，
Figure SMS_81
表示姿态角变化率和旋转角速度之间的关系系数，
Figure SMS_82
表示旋转角速度。
将无人机关于位置的方程和姿态角的变化率域机体旋转角之间的关系方程相结合，获取无人机关于欧拉角的运动学模型，表示为：
Figure SMS_83
（3）
更进一步的，基于上述对无人机机体运动的假设，使用牛顿-欧拉方程建立无人机的动力学模型。
应说明的是，由于假设了四旋翼飞行器是均匀对称的刚体，所以旋翼的合力（总拉 力）
Figure SMS_84
过
Figure SMS_85
点，且力的方向始终是单向的，与
Figure SMS_86
轴的负方向一致。
具体的，基于牛顿第二定律分析四旋翼无人机的运动状态，获取其运动规律，满足公式：
Figure SMS_87
（4）
其中，
Figure SMS_88
表示无人机的质量，
Figure SMS_89
表示无人机在地球坐标系下的重力，
Figure SMS_90
表示无 人机在机体坐标系下受到的总拉力。
将两边同除质量
Figure SMS_91
, 进一步可得:
Figure SMS_92
（5）
其中，
Figure SMS_93
表示重力加速度。
为了便于研究, 将拉力转换至地球坐标系下表示, 左乘旋转矩阵
Figure SMS_94
, 同时又考 虑到无人机此时的重力和拉力方向都是沿着
Figure SMS_95
轴的；可得位置动力学模型方程，表示 为:
Figure SMS_96
（6）
其中,
Figure SMS_97
是沿着地球坐标系中
Figure SMS_98
轴的单位向量，
Figure SMS_99
表示旋转矩阵。
更进一步的，无人机机体在合力矩
Figure SMS_100
的作用下以角速度
Figure SMS_101
，角加速度
Figure SMS_102
绕质心旋转 的欧拉方程，表示为：
Figure SMS_103
（7）
其中，
Figure SMS_104
表示为无人机转动惯量，
Figure SMS_105
表示为无人机合力矩，
Figure SMS_106
表示为无人机转动角加 速度，
Figure SMS_107
表示无人机转动角速度。
更进一步的，在机体坐标系内建立有关四旋翼无人机姿态的动力学关系满足:
Figure SMS_108
（8）
其中，
Figure SMS_110
为四旋翼无人机在机体坐标系的角速度;
Figure SMS_112
表示来自于陀螺转轴的角 速度而产生的陀螺力矩;
Figure SMS_115
包括旋翼绕
Figure SMS_111
轴、
Figure SMS_113
轴和
Figure SMS_116
轴分别产生的滚转力矩
Figure SMS_117
、俯仰力矩
Figure SMS_109
和偏航力矩
Figure SMS_114
。
更进一步的，联立位置动力学模型方程和姿态动力学模型方程以获取无人机动力学模型方程，表示为：
Figure SMS_118
（9）
更进一步的，联立无人机运动学模型方程和动力学模型方程获取无人机系统的数学模型，并对模型进行简化。
应说明的是，由无人机运动学模型方程和动力学模型方程联立所得的无人机系统的数学模型较为复杂，不利于控制器的设计与开发，因此需要对无人机系统的非线性数学模型进行进一步的简化。
具体的，假设无人机只进行小角度机动 (类似于平衡悬停态), 具有很小的俯仰角和滚转角, 因此可将总拉力近似地考虑为无人机的重力, 在此假设下,对无人机系统模型进行线性化处理，即假设：
Figure SMS_119
（10）
更进一步的，忽略
Figure SMS_120
，将无人机系统模型简化为：
Figure SMS_121
（11）
更进一步的，将变化矩阵
Figure SMS_122
简化为单位矩阵
Figure SMS_123
，将
Figure SMS_124
简化为：
Figure SMS_125
（12）
更进一步的，将简化后的无人机数学模型进行解耦获得水平位置模型（x、y通道）、高度位置模型（z通道）和姿态模型三个线性模型。
应说明的是，水平位置通道模型中包含
Figure SMS_126
两个通道的模型, 由于这两个通道的 推导完全类似, 可使用同一个线性模型进行表示。
具体的，水平位置模型表示为：
Figure SMS_127
（13）
其中，
Figure SMS_128
表示水平位置变化率（即水平速度），
Figure SMS_129
表示水平方向速度，
Figure SMS_130
表示水平 方向速度的变化率（即水平加速度），
Figure SMS_131
表示重力加速度，
Figure SMS_132
表示转换矩阵，
Figure SMS_133
表示滚转角 和俯仰角。
高度位置模型表示为：
Figure SMS_134
（14）
其中，
Figure SMS_135
表示竖直位置变化率（即竖直速度），
Figure SMS_136
表示竖直方向速度，
Figure SMS_137
表示竖直方 向速度的变化率（即水平加速度），
Figure SMS_138
表示重力加速度，
Figure SMS_139
表示螺旋桨总拉力大小，
Figure SMS_140
表示无 人机的质量。
姿态模型表示为：
Figure SMS_141
（15）
其中，
Figure SMS_142
表示姿态角变化率，
Figure SMS_143
表示在机体坐标系的角速度，
Figure SMS_144
表示无人机动转动 惯量，
Figure SMS_145
表示在机体坐标系的角速度变化率，
Figure SMS_146
表示螺旋桨在机体轴上产生的力矩。
S2：基于不确定性和扰动估计器UDE的基本设计原理和所述无人机系统数学模型建立非线性无人机系统模型，并基于所述非线性无人机系统模型设计UDE；
更进一步的，基于轨迹跟踪器UDE的基本设计原理和无人机系统数学模型进行建模，将非线性无人机系统数学模型表示为：
Figure SMS_147
（16）
其中,
Figure SMS_148
表示状态向量,
Figure SMS_149
为系统控制输入向量,
Figure SMS_150
为已知的状态矩阵,
Figure SMS_151
为列满秩的控制矩阵,
Figure SMS_152
为系统总 扰动。
Figure SMS_153
用于描述未知状态矩阵
Figure SMS_154
与无法预知的外界扰动
Figure SMS_155
所引起的系统总扰 动，表示为：
Figure SMS_156
（17）
其中，
Figure SMS_157
表示未知状态矩阵，
Figure SMS_158
表示外界扰动。
更进一步的，用参考模型来表示期望目标，表示为：
Figure SMS_159
（18）
其中，
Figure SMS_160
表示期望的输出状态。
系统的控制目标是将系统模型和参考模型间的误差
Figure SMS_161
控制到零, 其中误差
Figure SMS_162
表 示为：
Figure SMS_163
（19）
当控制的误差
Figure SMS_164
趋近于零时, 根据稳定性原理, 可以得出此时误差的微分方程 是稳定的，其中，误差的微分
Figure SMS_165
表示为：
Figure SMS_166
（20）
式中，
Figure SMS_167
为误差反馈增益。
应说明的是，稳定的参考模型其
Figure SMS_168
值一般为零；在实际应用中, 通常需要保证系 统鲁棒稳定性或者需要不同的误差微分方程时, 可以采用极点配置法等常用控制方法来 选择
Figure SMS_169
值大小。
更进一步的，综合公式（16）~（20）可得非线性无人机系统可表示为：
Figure SMS_170
（21）
则系统的控制输入向量
Figure SMS_171
可表示为:
Figure SMS_172
（22）
式中, 矩阵
Figure SMS_173
是矩阵
Figure SMS_174
的伪逆矩阵，定义为：
Figure SMS_175
。
更进一步的，将公式（22）中的控制律
Figure SMS_176
用拉普拉斯变换表示在
Figure SMS_177
域上, 表达 式为:
Figure SMS_178
（23）
由式（23）可知，控制律
Figure SMS_179
包括已知部分和未知部分，其中，已知部分为：
Figure SMS_180
；未知部分为：
Figure SMS_181
，包括系统 的不确定性和干扰。
更进一步的，用
Figure SMS_182
表示控制律
Figure SMS_183
中的未知部分，结合系统方程（16）可得
Figure SMS_184
的表达式为：
Figure SMS_185
（24）
假设
Figure SMS_186
是一种具有单位稳态增益和足够带宽的低通滤波器, 则UDE可用
Figure SMS_187
近似表示成：
Figure SMS_188
（25）
其中，UDE表示不确定性与扰动估计器。
由式（25）可知，系统不确定性和未知的外部干扰可通过输入的控制信号和系统的 状态来近似估计，因此可以将式（23）的
Figure SMS_189
改写为：
Figure SMS_190
（26）
更进一步的，得出基于UDE的控制方法为：
Figure SMS_191
（27）
应说明的是，UDE控制器是由系统的状态变量、反馈误差增益、可调参数的低通滤波器以及参考模型组成, 而系统模型参数的不确定性或未知的外部扰动在 UDE 控制器中不起作用。
常用的低通滤波器
Figure SMS_192
的表达式为:
Figure SMS_193
（28）
其中,
Figure SMS_194
是滤波器参数。
还应说明的是，UDE 控制器可通过减小
Figure SMS_195
来增加 UDE 的带宽, 使得系统总干扰
Figure SMS_196
的主要频带甚至是完整频带都被 UDE 的带宽包含, 从而保证
Figure SMS_197
能准确地估 计并补偿
Figure SMS_198
。
S3：利用极值搜索算法对所述UDE的控制器参数进行实时优化。
应说明的是，基于 UDE 的位置控制器可对外界扰动起到补偿作用, 但其补偿扰 动的效果由其滤波器参数
Figure SMS_199
决定,为了使 UDE 控制器更精准地对系统总干扰
Figure SMS_200
进 行估计并补偿, 使用极值搜索算法（ES 算法）对滤波器参数
Figure SMS_201
进行实时优化, 以便于提 高UDE控制器的性能。
具体的，如图2所示，设
Figure SMS_202
为输入控制信号, 即无人机的参考位置信号,
Figure SMS_203
为实际输出信号, 即无人机当前的实际位置信号；
Figure SMS_204
为代价函数，
Figure SMS_205
为待优化的 参数, 其中
Figure SMS_206
为迭代次数。
计算当前实际位置信号和参考位置信号的误差, 并将当前误差的平方进行离散时间上的积分, 其表达函数为：
Figure SMS_207
（29）
式中,
Figure SMS_208
表示某次计算的开始时刻,
Figure SMS_209
表示此次计算的结束时刻。
更进一步的，根据代价函数
Figure SMS_210
通过 ES 算法对 UDE 控制器的滤波器参数
Figure SMS_211
进行实时优化。
应说明的是，ES作为一种自适应算法，它使用由闭环系统的输出误差信号产生的成本函数迭代地改进控制器参数；其算法不基于具体模型，以函数的输出达到极值的方式迭代地改变函数的输入；通过扰动系统的输入变量，并由此估计输出梯度信息，然后利用该梯度信息在接下来的迭代中不断取到系统函数的极值，将待优化参数调整到最优。
还应说明的是，滤波器是UDE控制器对无人机跟踪控制的关键，其作用是能够对外界不确定性扰动进行估计以及补偿，但其滤波器参数设置无法实现对不同扰动的实时修改，经ES算法实时优化的滤波器参数能够对不同干扰的估计和补偿起到更好的效果。
实施例2
参照图1~5，为本发明的一个实施例，提供了一种基于极值搜索的无人机位置跟踪控制方法在具体场景下的应用。
本实施例所使用的四旋翼无人机为
Figure SMS_212
型常规结构，下面将在坐标系转换基础上, 通过动力学分析建立无人机的数学模型。
如图 3 所示, 地球坐标系
Figure SMS_213
用来表述无人机机体相对于地平面的位置 以及运动状态,
Figure SMS_214
轴垂直于地平面向下；机体坐标系
Figure SMS_215
的原点
Figure SMS_216
与四旋翼无 人机的重心位置相重合,
Figure SMS_217
轴为垂直四旋翼无人机的水平面向下, 每个电机产生的推 力
Figure SMS_218
和扭矩
Figure SMS_219
可以描述为：
Figure SMS_220
（30）
其中，
Figure SMS_221
是电机转速,
Figure SMS_222
是正推力常数,
Figure SMS_223
是正阻力常数。
沿四旋翼机体轴的总提升力
Figure SMS_224
和扭矩
Figure SMS_225
分别表示为：
Figure SMS_226
（31）
Figure SMS_227
其中，
Figure SMS_228
表示机臂长度, 即
Figure SMS_229
轴和电机轴之间的距离。
无人机相对于地球坐标系的位置和欧拉角由
Figure SMS_230
和
Figure SMS_231
表示。
假设无人机位置环境存在外界扰动
Figure SMS_232
, 根据地球坐标系和机体坐标 系的变换关系, 并通过小角度假设, 得到此时地球坐标系下无人机的动力学模型为：
Figure SMS_233
（32）
其中，
Figure SMS_235
是沿
Figure SMS_238
轴的无人机转动惯量,
Figure SMS_240
表示重力加速度,
Figure SMS_236
表 示无人机质量,
Figure SMS_239
表示螺旋桨的惯性,
Figure SMS_241
，
Figure SMS_242
，
Figure SMS_234
，
Figure SMS_237
分别表示俯仰 角，滚转角，偏航角的变化率。
四旋翼无人机的轨迹跟踪控制通过改变期望的位置实现, 将无人机的
Figure SMS_243
方 向的实际位置记为
Figure SMS_244
, 包括
Figure SMS_245
三个方向的位置, 参考信号为
Figure SMS_246
, 用以表示系 统状态
Figure SMS_247
的理想轨迹。
将式（17）中系统总扰动
Figure SMS_248
根据实际情况简化为二阶系统总扰动
Figure SMS_249
, 其表达式为：
Figure SMS_250
（33）
式中,
Figure SMS_251
表示系统的两个状态,
Figure SMS_252
表示有界且未知的常数,
Figure SMS_253
表示系统模型的不确定性,
Figure SMS_254
表示无法预知的外界扰动。
如果所有状态都是可测量的, 由于 PID 控制器具有较好控制性能, 对于系统 （33）, 以
Figure SMS_255
为例,
Figure SMS_256
定义为系统的期望输入，首先设计如下：
增加前馈项的
Figure SMS_257
控制器作为标称控制器:
Figure SMS_258
（34）
式中,
Figure SMS_259
为系统的前馈项, 其保证了系统未受到外界未知扰动时对轨迹跟踪 的有效性；
Figure SMS_260
为比例增益,
Figure SMS_261
为微分增益, 其作用是镇定系统并提高瞬态响应性能；
Figure SMS_262
是位置跟踪误差,
Figure SMS_263
是速度跟踪误差, 位置和速度跟踪误差定义为：
Figure SMS_264
（35）
根据式（34）, 在输出反馈控制器中加入UDE估计干扰, 用来补偿外界未知扰动,提高系统的控制性能。
将 UDE 控制器设计为：
Figure SMS_265
（36）
式中,
Figure SMS_266
为 UDE 用于消除系统干扰
Figure SMS_267
的估计值。
将上式（36）转换到频域中后得到：
Figure SMS_268
（37）
根据频域中的
Figure SMS_269
, 可知此信号的估计值与信号本身满足存在一定的滤波关系, 其估计信号
Figure SMS_270
表示为：
Figure SMS_271
（38）
进一步得到：
Figure SMS_272
（39）
将控制器
Figure SMS_273
代入可推导出经典 UDE 的设计：
Figure SMS_274
（40）
将滤波器（28）代入（40）即可得:
Figure SMS_275
（41）
最终可得其控制框图如图4所示。
具体的离散
Figure SMS_277
原理如图5所示；系统的待优化参数
Figure SMS_280
的估计值为
Figure SMS_282
, 使系 统输入估计值与附加的周期为
Figure SMS_278
且幅值为
Figure SMS_281
的扰动信号
Figure SMS_283
相叠加, 此时系统输 出为
Figure SMS_284
, 其输出的近似直流信号（基准信号）被高通 滤波器滤除, 其截止频率为
Figure SMS_276
, 在解调信号
Figure SMS_279
的作用下, 可得到系统函数的近 似梯度。
最后通过增益为
Figure SMS_285
的积分持续修改估计输入，直到系统函数到达极值点。
当系统函数到达极值点时，此时其成本函数为最小值，其系统函数的输出为待优化参数的最优解，便可将此参数用于无人机控制器的参数。
极值搜索算法使得系统函数收敛至极值点后，将获取到的最优参数用于控制器，然后对于新的系统参数再次执行极值搜索，将此过程不断迭代进行，实现对无人机跟踪控制参数的不断优化，最终实现更加稳定可靠的跟踪控制。
实施例3
参照图1~7，为本发明的一个实施例，提供了一种基于极值搜索的无人机位置跟踪控制方法，为了验证本发明的有益效果，通过仿真实验进行科学论证。
基于ROS的可视化平台RVIZ，通过在仿真过程中的无人机跟踪圆形轨迹时人为添加干扰信号，模拟无人机在有干扰情况下的飞行。
本实施例中采用一架四旋翼无人机在有干扰环境下，沿给定航迹，以1m/s的初始速度飞行，采样时间120s，其中仿真四旋翼无人机参数设置如表1所示。
表1四旋翼无人机参数设置
Figure SMS_286
仿真过程中，通过预先设置无人机跟踪的轨迹，加入干扰信号后，根据无人机实际航迹与设定航迹间的误差大小，验证UDE+ES算法的稳定性，并通过对有无极值搜索的UDE控制器进行比较，验证本文算法的有效性。实验结果如图6~7所示。
由图6~7可知，相较于传统的利用UDE进行轨迹跟踪控制的方法本发明提出的利用UDE+ES算法进行轨迹跟踪控制的方法能够更好的进行轨迹控制，使得无人机运行轨迹更加接近于预设轨迹；本发明所采用的UDE+ES算法能有效增强无人机轨迹跟踪器的抗干扰能力，减小不确定性和扰动带来的误差, 保证无人机稳定准确实现位置跟踪控制。
应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (2)

1.一种基于极值搜索的无人机位置跟踪控制方法，其特征在于，包括：
基于无人机机体运动规律构建无人机运动学模型为：
Figure FDA0004058809530000011
和无人机动力学模型为：/>
Figure FDA0004058809530000012
式中，pe表示无人机在地球坐标系的坐标位置，ve表示无人机在地球坐标系的速度，/>
Figure FDA0004058809530000013
表示姿态角变化率，W表示姿态角变化率和旋转角速度之间的关系系数，ωb表示旋转角速度，/>
Figure FDA0004058809530000014
表示无人机在地球坐标系的速度变化率即加速度，m表示无人机的质量，g表示重力加速度，e3是沿着地球坐标系中垂直于地平面向下的单位向量，/>
Figure FDA0004058809530000015
表示旋转矩阵，f表示旋翼的合力，J表示无人机转动惯量，Ga表示来自于陀螺转轴的角速度而产生的陀螺力矩，τ包括旋翼绕obxb轴、obyb轴和obzb轴分别产生的滚转力矩τx、俯仰力矩τy和偏航力矩τz；
解耦所述无人机系统数学模型为水平位置模型、高度位置模型和姿态模型，基于不确定性和扰动估计器UDE的基本设计原理和解耦所得模型建立非线性无人机系统模型为：Amx+Bmc(t)-Ax-Nx-Bu(t)-f(t)＝Ke；式中，Am为参考的状态矩阵，x＝(x1,...,xn)T是状态向量，Bm为参考的控制矩阵，c(t)＝[cr(t),...,cn(t)]T是期望的输出状态，A为已知的系统状态矩阵，N为未知的状态矩阵，B为已知的系统控制矩阵，u(t)＝[u1(t),...,un(t)]T为控制输入向量，f(t)为不确定的外部干扰，K为误差反馈增益，e为系统模型和参考模型间的误差；
基于所述非线性无人机系统模型获取控制律u(t)＝B+[Amx+Bmc(t)-Ax-Nx-f(t)-Ke]，将控制律用拉普拉斯变换表示在s域上,表示为：U(s)＝B+[AmX(s)+BmC(s)-KE(s)-AX(s)]+B+[-NX(s)-F(s)]；在轨迹跟踪器中加入低通滤波器，设计UDE为：UDE＝B+[(A-sI)X(s)+BU(s)]Gf(s)，并设计基于UDE的控制方法为：U(s)＝(I-B+BGf)-1B+·[AmX+BmC-KE-AX(1-Gf)-sGfX]，式中，Gf(s)表示低通滤波器，矩阵B+是矩阵B的伪逆矩阵；
构建代价函数
Figure FDA0004058809530000016
并通过极值搜索算法对UDE控制器的滤波器参数进行实时优化来控制实时扰动，式中,J(T(k))表示代价函数，t0表示某次计算的开始时刻,t表示此次计算的结束时刻。
2.如权利要求1所述的基于极值搜索的无人机位置跟踪控制方法，其特征在于：所述解耦无人机系统数学模型，包括：将无人机数学模型进行解耦获得水平位置模型、高度位置模型和姿态模型三个线性模型；
水平位置模型表示为：
Figure FDA0004058809530000021
其中，
Figure FDA0004058809530000022
表示水平位置变化率即水平速度，vh表示水平方向速度，/>
Figure FDA0004058809530000023
表示水平方向速度的变化率即水平加速度，g表示重力加速度，Aψ表示转换矩阵，Θh表示滚转角和俯仰角；
高度位置模型表示为：
Figure FDA0004058809530000024
其中，
Figure FDA0004058809530000025
表示竖直位置变化率即竖直速度，vz表示竖直方向速度，/>
Figure FDA0004058809530000026
表示竖直方向速度的变化率即水平加速度，g表示重力加速度，f表示螺旋桨总拉力大小，m表示无人机的质量；
姿态模型表示为：
Figure FDA0004058809530000027
其中，
Figure FDA0004058809530000028
表示姿态角变化率，ωb表示在机体坐标系的角速度，J表示无人机转动惯量，
Figure FDA0004058809530000029
表示在机体坐标系的角速度变化率，τ表示螺旋桨在机体轴上产生的力矩。/>
CN202211568382.9A 2022-12-08 2022-12-08 一种基于极值搜索的无人机位置跟踪控制方法 Active CN115576209B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211568382.9A CN115576209B (zh) 2022-12-08 2022-12-08 一种基于极值搜索的无人机位置跟踪控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211568382.9A CN115576209B (zh) 2022-12-08 2022-12-08 一种基于极值搜索的无人机位置跟踪控制方法

Publications (2)

Publication Number Publication Date
CN115576209A CN115576209A (zh) 2023-01-06
CN115576209B true CN115576209B (zh) 2023-03-31

Family

ID=84590669

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211568382.9A Active CN115576209B (zh) 2022-12-08 2022-12-08 一种基于极值搜索的无人机位置跟踪控制方法

Country Status (1)

Country Link
CN (1) CN115576209B (zh)

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103208000B (zh) * 2012-12-28 2015-10-21 青岛科技大学 基于局部极值快速搜索的特征点提取方法
CN103603767B (zh) * 2013-09-18 2016-03-30 国家电网公司 一种基于滑模的极值搜索控制参数自适应调整方法
CN106444799B (zh) * 2016-07-15 2019-03-12 浙江工业大学 基于模糊扩张状态观测器和自适应滑模的四旋翼无人机控制方法
CN106406325B (zh) * 2016-07-27 2019-05-31 浙江工业大学 基于模糊扩张状态观测器的四旋翼无人机反馈线性化控制方法
CN110705153B (zh) * 2019-09-24 2023-09-05 中国航空工业集团公司沈阳飞机设计研究所 一种无人机多阶安全系数取值方法
CN111027136A (zh) * 2019-11-25 2020-04-17 江苏大学 基于分数阶极值搜索的非线性abs控制方法
US11460822B2 (en) * 2019-12-11 2022-10-04 Johnson Controls Tyco IP Holdings LLP Self perturbing extremum-seeking control system
CN112363171B (zh) * 2020-08-18 2022-07-05 中国船舶重工集团公司第七一五研究所 一种解决被动警戒画面目标航迹交叉的自主跟踪方法
CN114723777A (zh) * 2020-12-21 2022-07-08 北京华航无线电测量研究所 一种基于嵌入式gpu平台的单目标跟踪方法
CN115129072A (zh) * 2022-03-16 2022-09-30 南京航空航天大学 固定翼无人机位置跟踪偏差约束下终端滑模控制方法

Also Published As

Publication number Publication date
CN115576209A (zh) 2023-01-06

Similar Documents

Publication Publication Date Title
He et al. A simple attitude control of quadrotor helicopter based on ziegler-nichols rules for tuning pd parameters
CN109062042B (zh) 一种旋翼飞行器的有限时间航迹跟踪控制方法
Huang et al. Fixed-time autonomous shipboard landing control of a helicopter with external disturbances
CN109116860B (zh) 三旋翼无人机的非线性鲁棒控制方法
CN111399531B (zh) 高超声速飞行器滑翔段制导与姿态控制一体化设计方法
CN104765272A (zh) 一种基于pid神经元网络控制(pidnn)的四旋翼飞行器控制方法
CN105929842A (zh) 一种基于动态速度调节的欠驱动uuv平面轨迹跟踪控制方法
CN112346470A (zh) 一种基于改进自抗扰控制的四旋翼姿态控制方法
CN111258216B (zh) 一种适用于四旋翼飞行器的滑模重复控制器
Liu et al. Robust command filtered adaptive backstepping control for a quadrotor aircraft
CN107491083A (zh) 一种基于饱和自适应滑模控制的四旋翼自主着船方法
Abas et al. Modeling and system identification using extended kalman filter for a quadrotor system
CN112631316B (zh) 变负载四旋翼无人机的有限时间控制方法
CN112558621A (zh) 一种基于解耦控制的飞行机械臂系统
Jiao et al. Analysis and design the controller for quadrotors based on PID control method
CN112578805A (zh) 一种旋翼飞行器的姿态控制方法
Sun et al. Nonlinear robust compensation method for trajectory tracking control of quadrotors
CN111522352A (zh) 多旋翼飞行器单参数自抗扰姿态控制器设计方法
CN109976364B (zh) 一种六旋翼飞行器姿态解耦控制方法
Fethalla et al. Robust observer-based backstepping controller for a quadrotor UAV
CN114489094A (zh) 一种基于加速度反馈增强的旋翼无人机抗风扰控制方法
CN115576209B (zh) 一种基于极值搜索的无人机位置跟踪控制方法
CN110134135B (zh) 基于改进mpc-pid的四旋翼飞行器控制方法
Liu et al. Quaternion-based robust attitude control for quadrotors
Xue et al. A moving target tracking control of quadrotor UAV based on passive control and super-twisting sliding mode control

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