CN110879611B - 无人机集群三维曲线路径跟踪方法及装置 - Google Patents

无人机集群三维曲线路径跟踪方法及装置 Download PDF

Info

Publication number
CN110879611B
CN110879611B CN201911058139.0A CN201911058139A CN110879611B CN 110879611 B CN110879611 B CN 110879611B CN 201911058139 A CN201911058139 A CN 201911058139A CN 110879611 B CN110879611 B CN 110879611B
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
rate
dimensional
path
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
CN201911058139.0A
Other languages
English (en)
Other versions
CN110879611A (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.)
China Academy of Electronic and Information Technology of CETC
Original Assignee
China Academy of Electronic and Information Technology of CETC
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 China Academy of Electronic and Information Technology of CETC filed Critical China Academy of Electronic and Information Technology of CETC
Priority to CN201911058139.0A priority Critical patent/CN110879611B/zh
Publication of CN110879611A publication Critical patent/CN110879611A/zh
Application granted granted Critical
Publication of CN110879611B publication Critical patent/CN110879611B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • G05D1/104—Simultaneous control of position or course in three dimensions specially adapted for aircraft involving a plurality of aircrafts, e.g. formation flying

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种无人机集群三维曲线路径跟踪方法及装置，所述方法包括：对每个无人机需要跟踪的三维空间中的一条一般曲线路径，根据所述一般曲线路径的隐函数方程组，确定所述一般曲线路径的可全局收敛到曲线路径上的组合向量场；根据所述组合向量场确定期望飞行航迹角及其变化率，并根据所述期望飞行航迹角及其变化率为无人机三维曲线路径跟踪设计带有反馈项和前馈项的饱和飞行航迹角变化率控制器，以满足无人机的转向率和爬升率约束；用对地转向角与对地转向率代替原始的转向角和转向率，进行无人机集群三维曲线路径跟踪。

Description

无人机集群三维曲线路径跟踪方法及装置
技术领域
本发明涉及无人机跟踪技术领域，尤其涉及一种无人机集群三维曲线路径跟踪方法及装置。
背景技术
在无人机集群的电力巡线、边界巡逻等应用中，通常会要求集群中无人机跟踪一条三维曲线路径飞行(如电线、边境线等)，而不对跟踪的时效性做出要求。实际任务中，可使无人机保持巡航速度飞行，仅通过改变其转向角和爬升角实现三维曲线路径跟踪。
涉及的主要问题是转向角变化率(即转向率)和爬升角变化率(即爬升率)的控制输入值设计。设计的难点包括：
1)不同于一条直线或一个圆的简单性，为三维空间中一条复杂的一般曲线路径的跟踪设计控制律是困难的；
2)无论是固定翼无人机还是旋翼无人机，其转向率和爬升率都受到平台性能的限制，不可能无限大；
3)无人机在飞行过程中受到风扰的影响。
已有的技术发明存在的主要缺点如下：
1)待跟踪路径均为具体的、简单的，多为二维直线、二维圆形、三维直线中的一种，而非三维空间中的一般曲线路径。
2)路径跟踪方法多是基于纯追踪或视线的方法，这些方法较难实现、精度较低、控制代价较高。
发明内容
本发明实施例提供一种无人机集群三维曲线路径跟踪方法及装置，用以解决现有技术中如何为无人机设计控制律以实现对给定的三维曲线路径进行跟踪的问题。
本发明实施例提供一种无人机集群三维曲线路径跟踪方法，包括：
对每个无人机需要跟踪的三维空间中的一条一般曲线路径，根据所述一般曲线路径的隐函数方程组，确定所述一般曲线路径的可全局收敛到曲线路径上的组合向量场，其中，所述无人机装备有能输出自身的位置和速度信息的导航模块；
根据所述组合向量场确定期望飞行航迹角及其变化率，并根据所述期望飞行航迹角及其变化率为无人机三维曲线路径跟踪设计带有反馈项和前馈项的饱和飞行航迹角变化率控制器，以满足无人机的转向率和爬升率约束；
用对地转向角与对地转向率代替原始的转向角和转向率，进行无人机集群三维曲线路径跟踪，以消除风扰的影响，其中，风速小于无人机的空速，风速只有水平分量，没有垂直分量。
本发明实施例还提供一种无人机集群三维曲线路径跟踪装置，包括：
组合向量场模块，用于对每个无人机需要跟踪的三维空间中的一条一般曲线路径，根据所述一般曲线路径的隐函数方程组，确定所述一般曲线路径的可全局收敛到曲线路径上的组合向量场，其中，所述无人机装备有能输出自身的位置和速度信息的导航模块；
控制器设置模块，用于根据所述组合向量场确定期望飞行航迹角及其变化率，并根据所述期望飞行航迹角及其变化率为无人机三维曲线路径跟踪设计带有反馈项和前馈项的饱和飞行航迹角变化率控制器，以满足无人机的转向率和爬升率约束；
替换模块，用于用对地转向角与对地转向率代替原始的转向角和转向率，进行无人机集群三维曲线路径跟踪，以消除风扰的影响，其中，风速小于无人机的空速，风速只有水平分量，没有垂直分量。
本发明实施例还提供一种无人机集群三维曲线路径跟踪装置，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现上述无人机集群三维曲线路径跟踪方法的步骤。
本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有信息传递的实现程序，所述程序被处理器执行时实现上述无人机集群三维曲线路径跟踪方法的步骤。
采用本发明实施例，能实现无人机对三维一般曲线路径的跟踪，可以跟踪三维空间中很复杂的曲线路径，不再局限于二维空间的路径、具体简单路径。本发明实施例的技术方案属于向量场法，具有易实现、精度高、控制代价低的优势。
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。
附图说明
通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：
图1是本发明实施例的无人机集群三维曲线路径跟踪方法的流程图；
图2是本发明实施例的组合向量场到期望三维光滑星型路径的收敛示例的示意图；
图3是本发明实施例的无人机三维光滑星型路径跟踪的飞行轨迹示例的示意图；
图4是本发明实施例的无人机三维光滑星型路径跟踪的控制输入示例的示意图；
图5a是本发明实施例的由4架无人机组成的无人机集群沿三维光滑星型曲线飞行在30秒时刻位置的示意图；
图5b是本发明实施例的由4架无人机组成的无人机集群沿三维光滑星型曲线飞行在50秒时刻位置的示意图；
图5c是本发明实施例的由4架无人机组成的无人机集群沿三维光滑星型曲线飞行在70秒时刻位置的示意图；
图5d是本发明实施例的由4架无人机组成的无人机集群沿三维光滑星型曲线飞行在120秒时刻位置的示意图；
图6是本发明一个实施例的无人机集群三维曲线路径跟踪装置的示意图；
图7是本发明另一个实施例的无人机集群三维曲线路径跟踪装置的示意图。
具体实施方式
下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。
首先对本发明实施例中涉及到的技术名词进行解释说明。
空速：无人机相对于空气的速度。
地速：考虑风扰的存在，无人机相对于当地地面的速度。
对地转向角/率：考虑风扰的存在，无人机相对于当地地面的转向角/率。
飞行航迹角：包括无人机的转向角和爬升角。
飞行航迹角变化率：包括无人机的转向角关于时间的导数(转向率)和爬升角关于时间的导数(爬升率)。
根据本发明实施例，提供了一种无人机集群三维曲线路径跟踪方法，图1是本发明实施例的无人机集群三维曲线路径跟踪方法的流程图，如图1所示，根据本发明实施的无人机集群三维曲线路径跟踪方法具体包括：
步骤101，对每个无人机需要跟踪的三维空间中的一条一般曲线路径，根据所述一般曲线路径的隐函数方程组，确定所述一般曲线路径的可全局收敛到曲线路径上的组合向量场，其中，所述无人机装备有能输出自身的位置和速度信息的导航模块；具体地：
由隐函数方程组{f(x,y,z)＝0,g(x,y,z)＝0}确定由两个曲面相交而成的二次可微的三维一般曲线路径
Figure BDA0002257110140000051
其中(x,y,z)为空间三维坐标，且函数f(x,y,z)和g(x,y,z)关于x、y、z的所有一阶和二阶偏导数均存在，f(x,y,z)和g(x,y,z)的数值尺度即数量级一致；
对二次可微的三维一般曲线路径
Figure BDA0002257110140000052
构造如下组合向量场：
Figure BDA0002257110140000053
Figure BDA0002257110140000054
Figure BDA0002257110140000055
其中，
Figure BDA0002257110140000056
表示x对时间的导数，，下标d表示期望达到的值，vd为无人机期望达到的速度，Vg为无人机的地速，
Figure BDA0002257110140000057
表示无人机当前位置(x,y,z)到曲线路径的距离，vc和vs分别为组成组合向量场的无旋向量场分量和螺线向量场分量，fx表示函数f(x,y,z)关于x的偏导数，其余类似；κ和s为组合向量场的两个可调整参数，κ＞0和s＝±1，其中，参数κ决定向量场收敛到三维一般曲线路径的速度，参数s决定向量场的收敛方向。
步骤102根据所述组合向量场确定期望飞行航迹角及其变化率，并根据所述期望飞行航迹角及其变化率为无人机三维曲线路径跟踪设计带有反馈项和前馈项的饱和飞行航迹角变化率控制器，以满足无人机的转向率和爬升率约束；具体地：
计算期望飞行航迹角，其包括期望对地转向角χd和期望爬升角γd：
Figure BDA0002257110140000061
Figure BDA0002257110140000062
其中，atan2()为四象限正切函数；
计算期望飞行航迹角变化率，其包括期望对地转向率
Figure BDA0002257110140000063
和期望爬升率
Figure BDA0002257110140000064
Figure BDA0002257110140000065
Figure BDA0002257110140000066
Figure BDA0002257110140000067
Figure BDA0002257110140000068
Figure BDA0002257110140000069
Figure BDA00022571101400000610
Figure BDA00022571101400000611
Figure BDA00022571101400000612
其中，Fl、Gl、Hl均是关于f、fx、fy、fz、fxy、fxz、fyy、fyz、fzz、g、gx、gy、gz、gxy、gxz、gyy、gyz、gzz的函数，l＝1,2,3。
设无人机的对地转向率和爬升率限制分别为
Figure BDA00022571101400000613
和
Figure BDA00022571101400000614
为使无人机在满足上述限制的同时实现对三维一般曲线路径的跟踪，分别为无人机的对地转向率和爬升率设计如下饱和控制器：
Figure BDA0002257110140000071
Figure BDA0002257110140000072
其中，sat()为饱和函数，kχ＞0和kγ＞0为反馈增益，
Figure BDA0002257110140000073
Figure BDA0002257110140000074
χe＝<χ-χd>、γe＝<γ-γd>表示无人机的飞行航迹角与期望飞行航迹角的误差，算子<>表示将误差角度等价转换到区间(-π,π]。
步骤103，用对地转向角与对地转向率代替原始的转向角和转向率，进行无人机集群三维曲线路径跟踪，以消除风扰的影响，其中，风速小于无人机的空速，风速只有水平分量，没有垂直分量。
以下结合附图，对本发明实施例的上述技术方案进行详细说明。
现有技术中的待跟踪路径均为具体的、简单的，多为二维直线、二维圆形、三维直线中的一种，而非三维空间中的一般曲线；且路径跟踪方法多是基于纯追踪或视线的方法，因此，本发明实施例提供一种无人机集群三维曲线路径跟踪方法。
本发明实施例的技术方案建立在以下假设的基础上：
无人机集群中的每个无人机装备有导航模块，能输出自身的位置、速度信息；装备有执行单元，能快速响应控制输入。
风速小于无人机的空速。
风速只有水平分量，没有垂直分量。
待跟踪的三维一般曲线路径
Figure BDA0002257110140000075
是二次可微的。二次可微曲线的具体定义如下：设
Figure BDA0002257110140000076
由两个曲面相交而成，即它可由隐函数方程组{f(x,y,z)＝0,g(x,y,z)＝0}确定，其中(x,y,z)为空间三维坐标。称
Figure BDA0002257110140000077
为二次可微的，如果函数f(x,y,z)和g(x,y,z)关于x、y、z的所有一阶和二阶偏导数均存在。
f(x,y,z)和g(x,y,z)的数值尺度(数量级)是相当的，差别不大。
基于如上假设，本发明实施例的技术方案对每个无人机要跟踪的三维空间中一条一般曲线路径，根据其隐函数方程组表达式，为其设计了可全局收敛到曲线路径上的组合向量场。根据组合向量场决定的期望飞行航迹角及其变化率，为无人机三维曲线路径跟踪设计了带有反馈项和前馈项的饱和飞行航迹角变化率控制器，能满足无人机的转向率和爬升率约束。用对地转向角与对地转向率代替原始的转向角和转向率，来消除风扰的影响。
本发明实施例的技术方案为“无人机对三维空间中的一般曲线路径进行跟踪的问题”提供一种基于所发明组合向量场的易实现、精度高、控制代价低的解决方案，该方案不局限于二维空间的路径、不局限于具体路径，能处理三维空间中的一般曲线路径，能有效处理无人机的飞行性能约束，能有效处理飞行环境中风扰的影响。
以下对本发明实施例进行详细说明。
步骤一.构建组合向量场
根据亥姆霍兹定理，任意足够光滑、快速衰减的向量场均可分解为一个无旋向量场和一个螺线向量场的和。据此，为二次可微的三维一般曲线路径
Figure BDA0002257110140000082
构造如下组合向量场：
Figure BDA0002257110140000081
其中
Figure BDA0002257110140000091
Figure BDA0002257110140000092
其中，
Figure BDA0002257110140000093
表示x对时间的导数(下同)，下标d表示期望达到的值(下同)，Vg为无人机的地速，
Figure BDA0002257110140000094
可用来表示无人机当前位置(x,y,z)到曲线路径的距离，vc和vs分别为组成公式1所示组合向量场的无旋向量场分量和螺线向量场分量，fx表示函数f(x,y,z)关于x的偏导数(下同)。公式1所示组合向量场包含两个可调整参数，κ＞0和s＝±1。其中参数κ决定向量场收敛到三维一般曲线路径的速度，κ越大，收敛速度越快，也意味着对飞行航迹角变化率的期望值越大；参数s决定向量场的收敛方向(顺时针、逆时针等)。
由公式1定义的组合向量场可全局收敛到由隐函数方程组{f(x,y,z)＝0,g(x,y,z)＝0}确定的三维一般曲线路径。
步骤二.计算期望飞行航迹角及其变化率
期望飞行航迹角包括期望对地转向角χd和期望爬升角γd，它们分别计算如下：
Figure BDA0002257110140000095
Figure BDA0002257110140000096
其中，atan2()为四象限正切函数；
计算期望飞行航迹角变化率，其包括期望对地转向率
Figure BDA0002257110140000097
和期望爬升率
Figure BDA0002257110140000098
Figure BDA0002257110140000099
Figure BDA00022571101400000910
Figure BDA00022571101400000911
Figure BDA00022571101400000912
Figure BDA00022571101400000913
Figure BDA0002257110140000101
Figure BDA0002257110140000102
Figure BDA0002257110140000103
其中，Fl、Gl、Hl(l＝1,2,3)均是关于f、fx、fy、fz、fxy、fxz、fyy、fyz、fzz、g、gx、gy、gz、gxy、gxz、gyy、gyz、gzz等的函数，具体略。
由公式6、7定义的期望飞行航迹角变化率中隐含无人机的飞行航迹角与组合向量场决定的期望飞行航迹角之间的误差，能引导无人机的飞行航迹逐渐收敛到由组合向量场决定的期望飞行航迹，最终实现对三维一般曲线路径的跟踪。
步骤三.计算饱和控制律
设无人机的对地转向率和爬升率限制分别为
Figure BDA0002257110140000104
和
Figure BDA0002257110140000105
为使无人机在满足上述限制的同时实现对三维一般曲线路径的跟踪，分别对无人机的对地转向率和爬升率设计如下饱和控制器：
Figure BDA0002257110140000106
Figure BDA0002257110140000107
其中，sat()为饱和函数，kχ＞0和kγ＞0为反馈增益，
Figure BDA0002257110140000108
Figure BDA0002257110140000109
χe＝<χ-χd>、γe＝<γ-γd>表示无人机的飞行航迹角与期望飞行航迹角的误差，算子<>表示将误差角度等价转换到区间(-π,π]。
图2为本发明中当可调参数取不同值时，所构建的组合向量场收敛到期望三维光滑星型路径的示例。
图3为本发明中所述方法用于三维光滑星型路径跟踪时无人机飞行轨迹的示例。
图4为本发明中所述方法用于三维光滑星型路径跟踪时无人机控制输入值与无人机飞行性能约束值
Figure BDA0002257110140000111
比较的示例。
图5为本发明实施例的技术方案中所述方法用于由4架无人机组成的无人机集群沿三维光滑星型曲线飞行的示例。其中，浅色圆形、三角形、菱形、五角星分别为4架无人机初始时刻的位置；深色圆形、三角形、菱形、五角星分别为4架无人机在30秒(图5a)、50秒(图5b)、70秒(图5c)、120秒(图5d，俯视图)时刻的位置；曲线为4架无人机的飞行轨迹。可以看到，每架无人机的飞行轨迹均收敛到一条光滑星型曲线。
综上所述，本发明实施例的技术方案能实现无人机对三维一般曲线路径的跟踪，可以跟踪三维空间中很复杂的曲线路径，不再局限于二维空间的路径、具体简单路径。本发明实施例的技术方案属于向量场法，具有易实现、精度高、控制代价低的优势。
根据本发明实施例，提供了一种无人机集群三维曲线路径跟踪装置，图6是本发明实施例的无人机集群三维曲线路径跟踪装置的示意图，如图6所示，根据本发明实施例的无人机集群三维曲线路径跟踪装置具体包括：
组合向量场模块60，用于对每个无人机需要跟踪的三维空间中的一条一般曲线路径，根据所述一般曲线路径的隐函数方程组，确定所述一般曲线路径的可全局收敛到曲线路径上的组合向量场，其中，所述无人机装备有能输出自身的位置和速度信息的导航模块；所述组合向量场模块60具体用于：
由隐函数方程组{f(x,y,z)＝0,g(x,y,z)＝0}确定由两个曲面相交而成的二次可微的三维一般曲线路径
Figure BDA0002257110140000112
其中(x,y,z)为空间三维坐标，且函数f(x,y,z)和g(x,y,z)关于x、y、z的所有一阶和二阶偏导数均存在，f(x,y,z)和g(x,y,z)的数值尺度即数量级一致；
对二次可微的三维一般曲线路径
Figure BDA0002257110140000113
构造如下组合向量场：
Figure BDA0002257110140000121
Figure BDA0002257110140000122
Figure BDA0002257110140000123
其中，
Figure BDA0002257110140000124
表示x对时间的导数，下标d表示期望达到的值，vd为无人机期望达到的速度，Vg为无人机的地速，
Figure BDA0002257110140000125
表示无人机当前位置(x,y,z)到曲线路径的距离，vc和vs分别为组成组合向量场的无旋向量场分量和螺线向量场分量，fx表示函数f(x,y,z)关于x的偏导数，其余类似；κ和s为组合向量场的两个可调整参数，κ＞0和s＝±1，其中，参数κ决定向量场收敛到三维一般曲线路径的速度，参数s决定向量场的收敛方向。
控制器设置模块62，用于根据所述组合向量场确定期望飞行航迹角及其变化率，并根据所述期望飞行航迹角及其变化率为无人机三维曲线路径跟踪设计带有反馈项和前馈项的饱和飞行航迹角变化率控制器，以满足无人机的转向率和爬升率约束；所述控制器设置模块62具体用于：
计算期望飞行航迹角，其包括期望对地转向角χd和期望爬升角γd：
Figure BDA0002257110140000126
Figure BDA0002257110140000127
其中，atan2()为四象限正切函数；
计算期望飞行航迹角变化率，其包括期望对地转向率
Figure BDA0002257110140000128
和期望爬升率
Figure BDA0002257110140000129
Figure BDA00022571101400001210
Figure BDA0002257110140000131
Figure BDA0002257110140000132
Figure BDA0002257110140000133
Figure BDA0002257110140000134
Figure BDA0002257110140000135
Figure BDA0002257110140000136
Figure BDA0002257110140000137
其中，Fl、Gl、Hl均是关于f、fx、fy、fz、fxy、fxz、fyy、fyz、fzz、g、gx、gy、gz、gxy、gxz、gyy、gyz、gzz的函数，l＝1,2,3。
所述控制器设置模块62具体用于：
设无人机的对地转向率和爬升率限制分别为
Figure BDA0002257110140000138
和
Figure BDA0002257110140000139
为使无人机在满足上述限制的同时实现对三维一般曲线路径的跟踪，分别为无人机的对地转向率和爬升率设计如下饱和控制器：
Figure BDA00022571101400001310
Figure BDA00022571101400001311
其中，sat()为饱和函数，kχ＞0和kγ＞0为反馈增益，
Figure BDA00022571101400001312
Figure BDA00022571101400001313
χe＝<χ-χd>、γe＝<γ-γd>表示无人机的飞行航迹角与期望飞行航迹角的误差，算子<>表示将误差角度等价转换到区间(-π,π]。
替换模块64，用于用对地转向角与对地转向率代替原始的转向角和转向率，进行无人机集群三维曲线路径跟踪，以消除风扰的影响，其中，风速小于无人机的空速，风速只有水平分量，没有垂直分量。
综上所述，本发明实施例的技术方案能实现无人机对三维一般曲线路径的跟踪，可以跟踪三维空间中很复杂的曲线路径，不再局限于二维空间的路径、具体简单路径。本发明实施例的技术方案属于向量场法，具有易实现、精度高、控制代价低的优势。
本发明实施例提供一种无人机集群三维曲线路径跟踪装置，如图7所示，包括：存储器70、处理器72及存储在所述存储器70上并可在所述处理器72上运行的计算机程序，所述计算机程序被所述处理器72执行时实现如下方法步骤：
步骤101，对每个无人机需要跟踪的三维空间中的一条一般曲线路径，根据所述一般曲线路径的隐函数方程组，确定所述一般曲线路径的可全局收敛到曲线路径上的组合向量场，其中，所述无人机装备有能输出自身的位置和速度信息的导航模块；具体地：
由隐函数方程组{f(x,y,z)＝0,g(x,y,z)＝0}确定由两个曲面相交而成的二次可微的三维一般曲线路径
Figure BDA0002257110140000141
其中(x,y,z)为空间三维坐标，且函数f(x,y,z)和g(x,y,z)关于x、y、z的所有一阶和二阶偏导数均存在，f(x,y,z)和g(x,y,z)的数值尺度即数量级一致；
对二次可微的三维一般曲线路径
Figure BDA0002257110140000142
构造如下组合向量场：
Figure BDA0002257110140000143
Figure BDA0002257110140000151
Figure BDA0002257110140000152
其中，
Figure BDA0002257110140000153
表示x对时间的导数，下标d表示期望达到的值，vd为无人机期望达到的速度，Vg为无人机的地速，
Figure BDA0002257110140000154
表示无人机当前位置(x,y,z)到曲线路径的距离，vc和vs分别为组成组合向量场的无旋向量场分量和螺线向量场分量，fx表示函数f(x,y,z)关于x的偏导数，其余类似；κ和s为组合向量场的两个可调整参数，κ＞0和s＝±1，其中，参数κ决定向量场收敛到三维一般曲线路径的速度，参数s决定向量场的收敛方向。
步骤102根据所述组合向量场确定期望飞行航迹角及其变化率，并根据所述期望飞行航迹角及其变化率为无人机三维曲线路径跟踪设计带有反馈项和前馈项的饱和飞行航迹角变化率控制器，以满足无人机的转向率和爬升率约束；具体地：
计算期望飞行航迹角，其包括期望对地转向角χd和期望爬升角γd：
Figure BDA0002257110140000155
Figure BDA0002257110140000156
其中，atan2()为四象限正切函数；
计算期望飞行航迹角变化率，其包括期望对地转向率
Figure BDA0002257110140000157
和期望爬升率
Figure BDA0002257110140000158
Figure BDA0002257110140000159
Figure BDA00022571101400001510
Figure BDA00022571101400001511
Figure BDA00022571101400001512
Figure BDA00022571101400001513
Figure BDA0002257110140000161
Figure BDA0002257110140000162
Figure BDA0002257110140000163
其中，Fl、Gl、Hl均是关于f、fx、fy、fz、fxy、fxz、fyy、fyz、fzz、g、gx、gy、gz、gxy、gxz、gyy、gyz、gzz的函数，l＝1,2,3。
设无人机的对地转向率和爬升率限制分别为
Figure BDA0002257110140000164
和
Figure BDA0002257110140000165
为使无人机在满足上述限制的同时实现对三维一般曲线路径的跟踪，分别为无人机的对地转向率和爬升率设计如下饱和控制器：
Figure BDA0002257110140000166
Figure BDA0002257110140000167
其中，sat()为饱和函数，kχ＞0和kγ＞0为反馈增益，
Figure BDA0002257110140000168
Figure BDA0002257110140000169
χe＝<χ-χd>、γe＝<γ-γd>表示无人机的飞行航迹角与期望飞行航迹角的误差，算子<>表示将误差角度等价转换到区间(-π,π]。
步骤103，用对地转向角与对地转向率代替原始的转向角和转向率，进行无人机集群三维曲线路径跟踪，以消除风扰的影响，其中，风速小于无人机的空速，风速只有水平分量，没有垂直分量。
本发明实施例提供一种计算机可读存储介质，所述计算机可读存储介质上存储有信息传输的实现程序，所述程序被处理器72执行时实现如下方法步骤：
步骤101，对每个无人机需要跟踪的三维空间中的一条一般曲线路径，根据所述一般曲线路径的隐函数方程组，确定所述一般曲线路径的可全局收敛到曲线路径上的组合向量场，其中，所述无人机装备有能输出自身的位置和速度信息的导航模块；具体地：
由隐函数方程组{f(x,y,z)＝0,g(x,y,z)＝0}确定由两个曲面相交而成的二次可微的三维一般曲线路径
Figure BDA0002257110140000171
其中(x,y,z)为空间三维坐标，且函数f(x,y,z)和g(x,y,z)关于x、y、z的所有一阶和二阶偏导数均存在，f(x,y,z)和g(x,y,z)的数值尺度即数量级一致；
对二次可微的三维一般曲线路径
Figure BDA0002257110140000172
构造如下组合向量场：
Figure BDA0002257110140000173
Figure BDA0002257110140000174
Figure BDA0002257110140000175
其中，
Figure BDA0002257110140000176
表示x对时间的导数，下标d表示期望达到的值，vd为无人机期望达到的速度，Vg为无人机的地速，
Figure BDA0002257110140000177
表示无人机当前位置(x,y,z)到曲线路径的距离，vc和vs分别为组成组合向量场的无旋向量场分量和螺线向量场分量，fx表示函数f(x,y,z)关于x的偏导数，其余类似；κ和s为组合向量场的两个可调整参数，κ＞0和s＝±1，其中，参数κ决定向量场收敛到三维一般曲线路径的速度，参数s决定向量场的收敛方向。
步骤102根据所述组合向量场确定期望飞行航迹角及其变化率，并根据所述期望飞行航迹角及其变化率为无人机三维曲线路径跟踪设计带有反馈项和前馈项的饱和飞行航迹角变化率控制器，以满足无人机的转向率和爬升率约束；具体地：
计算期望飞行航迹角，其包括期望对地转向角χd和期望爬升角γd：
Figure BDA0002257110140000181
Figure BDA0002257110140000182
其中，atan2()为四象限正切函数；
计算期望飞行航迹角变化率，其包括期望对地转向率
Figure BDA0002257110140000183
和期望爬升率
Figure BDA0002257110140000184
Figure BDA0002257110140000185
Figure BDA0002257110140000186
Figure BDA0002257110140000187
Figure BDA0002257110140000188
Figure BDA0002257110140000189
Figure BDA00022571101400001810
Figure BDA00022571101400001811
Figure BDA00022571101400001812
其中，Fl、Gl、Hl均是关于f、fx、fy、fz、fxy、fxz、fyy、fyz、fzz、g、gx、gy、gz、gxy、gxz、gyy、gyz、gzz的函数，l＝1,2,3。
设无人机的对地转向率和爬升率限制分别为
Figure BDA00022571101400001813
和
Figure BDA00022571101400001814
为使无人机在满足上述限制的同时实现对三维一般曲线路径的跟踪，分别为无人机的对地转向率和爬升率设计如下饱和控制器：
Figure BDA00022571101400001815
Figure BDA0002257110140000191
其中，sat()为饱和函数，kχ＞0和kγ＞0为反馈增益，
Figure BDA0002257110140000192
Figure BDA0002257110140000193
χe＝<χ-χd>、γe＝<γ-γd>表示无人机的飞行航迹角与期望飞行航迹角的误差，算子<>表示将误差角度等价转换到区间(-π,π]。
步骤103，用对地转向角与对地转向率代替原始的转向角和转向率，进行无人机集群三维曲线路径跟踪，以消除风扰的影响，其中，风速小于无人机的空速，风速只有水平分量，没有垂直分量。
本实施例所述计算机可读存储介质包括但不限于为：ROM、RAM、磁盘或光盘等。
显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种无人机集群三维曲线路径跟踪方法，其特征在于，包括：
对每个无人机需要跟踪的三维空间中的一条一般曲线路径，根据所述一般曲线路径的隐函数方程组，确定所述一般曲线路径的可全局收敛到曲线路径上的组合向量场，其中，所述无人机装备有能输出自身的位置和速度信息的导航模块；
根据所述组合向量场确定期望飞行航迹角及其变化率，并根据所述期望飞行航迹角及其变化率为无人机三维曲线路径跟踪设计带有反馈项和前馈项的饱和飞行航迹角变化率控制器，以满足无人机的转向率和爬升率约束；
用对地转向角与对地转向率代替原始的转向角和转向率，进行无人机集群三维曲线路径跟踪，以消除风扰的影响，其中，风速小于无人机的空速，风速只有水平分量，没有垂直分量。
2.如权利要求1所述的方法，其特征在于，对每个无人机需要跟踪的三维空间中的一条一般曲线路径，根据所述一般曲线路径的隐函数方程组，确定所述一般曲线路径的可全局收敛到曲线路径上的组合向量场，具体包括：
由隐函数方程组{f(x,y,z)＝0,g(x,y,z)＝0}确定由两个曲面相交而成的二次可微的三维一般曲线路径
Figure FDA0002257110130000011
其中(x,y,z)为空间三维坐标，且函数f(x,y,z)和g(x,y,z)关于x、y、z的所有一阶和二阶偏导数均存在，f(x,y,z)和g(x,y,z)的数值尺度即数量级一致；
对二次可微的三维一般曲线路径
Figure FDA0002257110130000012
构造如下组合向量场：
Figure FDA0002257110130000021
Figure FDA0002257110130000022
Figure FDA0002257110130000023
其中，
Figure FDA0002257110130000024
表示x对时间的导数，下标d表示期望达到的值，vd为无人机期望达到的速度，Vg为无人机的地速，
Figure FDA0002257110130000025
表示无人机当前位置(x,y,z)到曲线路径的距离，vc和vs分别为组成组合向量场的无旋向量场分量和螺线向量场分量，fx表示函数f(x,y,z)关于x的偏导数，其余类似；κ和s为组合向量场的两个可调整参数，κ＞0和s＝±1，其中，参数κ决定向量场收敛到三维一般曲线路径的速度，参数s决定向量场的收敛方向。
3.如权利要求2所述的方法，其特征在于，根据所述组合向量场确定期望飞行航迹角及其变化率，具体包括：
计算期望飞行航迹角，其包括期望对地转向角χd和期望爬升角γd：
Figure FDA0002257110130000026
Figure FDA0002257110130000027
其中，atan2()为四象限正切函数；
计算期望飞行航迹角变化率，其包括期望对地转向率
Figure FDA0002257110130000028
和期望爬升率
Figure FDA0002257110130000029
Figure FDA00022571101300000210
Figure FDA00022571101300000211
Figure FDA00022571101300000212
Figure FDA0002257110130000031
Figure FDA00022571101300000311
Figure FDA0002257110130000032
Figure FDA0002257110130000033
Figure FDA0002257110130000034
其中，Fl、Gl、Hl均是关于f、fx、fy、fz、fxy、fxz、fyy、fyz、fzz、g、gx、gy、gz、gxy、gxz、gyy、gyz、gzz的函数，l＝1,2,3。
4.如权利要求3所述的方法，其特征在于，根据所述期望飞行航迹角及其变化率为无人机三维曲线路径跟踪设计带有反馈项和前馈项的饱和飞行航迹角变化率控制器，具体包括：
设无人机的对地转向率和爬升率限制分别为
Figure FDA0002257110130000035
和
Figure FDA0002257110130000036
为使无人机在满足上述限制的同时实现对三维一般曲线路径的跟踪，分别为无人机的对地转向率和爬升率设计如下饱和控制器：
Figure FDA0002257110130000037
Figure FDA0002257110130000038
其中，sat()为饱和函数，kχ＞0和kγ＞0为反馈增益，
Figure FDA0002257110130000039
Figure FDA00022571101300000310
χe＝<χ-χd>、γe＝<γ-γd>表示无人机的飞行航迹角与期望飞行航迹角的误差，算子<>表示将误差角度等价转换到区间(-π,π]。
5.一种无人机集群三维曲线路径跟踪装置，其特征在于，包括：
组合向量场模块，用于对每个无人机需要跟踪的三维空间中的一条一般曲线路径，根据所述一般曲线路径的隐函数方程组，确定所述一般曲线路径的可全局收敛到曲线路径上的组合向量场，其中，所述无人机装备有能输出自身的位置和速度信息的导航模块；
控制器设置模块，用于根据所述组合向量场确定期望飞行航迹角及其变化率，并根据所述期望飞行航迹角及其变化率为无人机三维曲线路径跟踪设计带有反馈项和前馈项的饱和飞行航迹角变化率控制器，以满足无人机的转向率和爬升率约束；
替换模块，用于用对地转向角与对地转向率代替原始的转向角和转向率，进行无人机集群三维曲线路径跟踪，以消除风扰的影响，其中，风速小于无人机的空速，风速只有水平分量，没有垂直分量。
6.如权利要求5所述的装置，其特征在于，所述组合向量场模块具体用于：
由隐函数方程组{f(x,y,z)＝0,g(x,y,z)＝0}确定由两个曲面相交而成的二次可微的三维一般曲线路径
Figure FDA0002257110130000041
其中(x,y,z)为空间三维坐标，且函数f(x,y,z)和g(x,y,z)关于x、y、z的所有一阶和二阶偏导数均存在，f(x,y,z)和g(x,y,z)的数值尺度即数量级一致；
对二次可微的三维一般曲线路径
Figure FDA0002257110130000042
构造如下组合向量场：
Figure FDA0002257110130000043
Figure FDA0002257110130000044
Figure FDA0002257110130000051
其中，
Figure FDA0002257110130000052
表示x对时间的导数，下标d表示期望达到的值，vd为无人机期望达到的速度，Vg为无人机的地速，
Figure FDA0002257110130000053
表示无人机当前位置(x,y,z)到曲线路径的距离，vc和vs分别为组成组合向量场的无旋向量场分量和螺线向量场分量，fx表示函数f(x,y,z)关于x的偏导数，其余类似；κ和s为组合向量场的两个可调整参数，κ＞0和s＝±1，其中，参数κ决定向量场收敛到三维一般曲线路径的速度，参数s决定向量场的收敛方向。
7.如权利要求6所述的装置，其特征在于，所述控制器设置模块具体用于：
计算期望飞行航迹角，其包括期望对地转向角χd和期望爬升角γd：
Figure FDA0002257110130000054
Figure FDA0002257110130000055
其中，atan2()为四象限正切函数；
计算期望飞行航迹角变化率，其包括期望对地转向率
Figure FDA0002257110130000056
和期望爬升率
Figure FDA0002257110130000057
Figure FDA0002257110130000058
Figure FDA0002257110130000059
Figure FDA00022571101300000510
Figure FDA00022571101300000511
Figure FDA00022571101300000512
Figure FDA00022571101300000513
Figure FDA00022571101300000514
Figure FDA0002257110130000061
其中，Fl、Gl、Hl均是关于f、fx、fy、fz、fxy、fxz、fyy、fyz、fzz、g、gx、gy、gz、gxy、gxz、gyy、gyz、gzz的函数，l＝1,2,3。
8.如权利要求7所述的装置，其特征在于，所述控制器设置模块具体用于：
设无人机的对地转向率和爬升率限制分别为
Figure FDA0002257110130000062
和
Figure FDA0002257110130000063
为使无人机在满足上述限制的同时实现对三维一般曲线路径的跟踪，分别为无人机的对地转向率和爬升率设计如下饱和控制器：
Figure FDA0002257110130000064
Figure FDA0002257110130000065
其中，sat()为饱和函数，kχ＞0和kγ＞0为反馈增益，
Figure FDA0002257110130000066
Figure FDA0002257110130000067
χe＝<χ-χd>、γe＝<γ-γd>表示无人机的飞行航迹角与期望飞行航迹角的误差，算子<>表示将误差角度等价转换到区间(-π,π]。
9.一种无人机集群三维曲线路径跟踪装置，其特征在于，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如权利要求1至4中任一项所述的无人机集群三维曲线路径跟踪方法的步骤。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有信息传递的实现程序，所述程序被处理器执行时实现如权利要求1至4中任一项所述的无人机集群三维曲线路径跟踪方法的步骤。
CN201911058139.0A 2019-11-01 2019-11-01 无人机集群三维曲线路径跟踪方法及装置 Active CN110879611B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911058139.0A CN110879611B (zh) 2019-11-01 2019-11-01 无人机集群三维曲线路径跟踪方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911058139.0A CN110879611B (zh) 2019-11-01 2019-11-01 无人机集群三维曲线路径跟踪方法及装置

Publications (2)

Publication Number Publication Date
CN110879611A CN110879611A (zh) 2020-03-13
CN110879611B true CN110879611B (zh) 2022-12-16

Family

ID=69728215

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911058139.0A Active CN110879611B (zh) 2019-11-01 2019-11-01 无人机集群三维曲线路径跟踪方法及装置

Country Status (1)

Country Link
CN (1) CN110879611B (zh)

Family Cites Families (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
AU2010317648B2 (en) * 2009-11-10 2014-05-29 Bae Systems Australia Limited Method and system to aid craft movement prediction
EP3279610A1 (en) * 2016-08-01 2018-02-07 The Boeing Company Optimum cruise climb tracking for reduced fuel consumption using vertical and lateral navigation
CN106774400B (zh) * 2016-12-28 2020-02-07 北京航空航天大学 一种基于逆动力学的无人机三维轨迹制导方法
CN107798188B (zh) * 2017-10-25 2020-11-06 国网上海市电力公司 一种两根隧道电缆稳态温升快速获取方法
CN108318029A (zh) * 2017-11-27 2018-07-24 中国电子科技集团公司电子科学研究院 姿态跟踪与图像叠加方法及显示设备
CN108388270B (zh) * 2018-03-21 2021-08-31 天津大学 面向安全域的集群无人机轨迹姿态协同控制方法
CN108873929B (zh) * 2018-05-24 2020-05-29 北京航空航天大学 一种固定翼飞机自主着舰方法及系统
CN108663939A (zh) * 2018-06-12 2018-10-16 哈尔滨工程大学 考虑通讯丢包的uuv集群水平面路径跟踪控制方法
CN109782806B (zh) * 2019-02-19 2020-10-23 北京邮电大学 一种无人机室内路径跟踪方法及装置
CN110119158B (zh) * 2019-05-13 2020-08-18 浙江大学 一种高亚音速无人飞行器多机协同编队控制系统及方法
CN110262547B (zh) * 2019-06-21 2021-08-31 西安电子科技大学 一种基于路径跟踪的圆形编队起飞方法
CN110362115B (zh) * 2019-07-31 2022-02-18 中国人民解放军总参谋部第六十研究所 一种时间约束同时到达多无人机路径规划算法
KR20190110964A (ko) * 2019-09-10 2019-10-01 엘지전자 주식회사 탑승자 유형 검출 장치 및 방법

Also Published As

Publication number Publication date
CN110879611A (zh) 2020-03-13

Similar Documents

Publication Publication Date Title
Ansari et al. Quadrotor control via robust generalized dynamic inversion and adaptive non‐singular terminal sliding mode
Zheng et al. Planning and tracking in image space for image-based visual servoing of a quadrotor
Li et al. Robust tracking control strategy for a quadrotor using RPD-SMC and RISE
CN111338374B (zh) 无人机集群编队控制方法
He et al. Acceleration-feedback-enhanced robust control of an unmanned helicopter
CN114661069B (zh) 一种群体智能系统的编队控制方法
Cai et al. Design and implementation of a flight control system for an unmanned rotorcraft using RPT control approach
Fowler et al. Bézier curve path planning for parafoil terminal guidance
CN109507890A (zh) 一种基于eso的无人机动态逆广义预测控制器
CN112068586B (zh) 一种空间时间联合优化的四旋翼无人飞行器轨迹规划方法
CN110687920A (zh) 无人机集群编队控制方法、装置及储存介质
CN115639830B (zh) 一种空地智能体协同编队控制系统及其编队控制方法
CN114138010B (zh) 一种基于加权偏差的多智能体高阶滑模编队控制方法
CN111522356A (zh) 一种倾转旋翼无人机强鲁棒全包线一体化控制方法
Santos et al. Suspended Load Path Tracking Control Strategy Using a Tilt‐Rotor UAV
Wang et al. Robust trajectory planning for spatial-temporal multi-drone coordination in large scenes
Tao et al. A PID and fuzzy logic based method for Quadrotor aircraft control motion
El Houm et al. Optimal new sliding mode controller combined with modified supertwisting algorithm for a perturbed quadrotor UAV
CN110879611B (zh) 无人机集群三维曲线路径跟踪方法及装置
CN110618702B (zh) 一种无人机集群三维曲线路径跟踪方法、装置和存储介质
Di et al. Low-level control with actuator dynamics for multirotor UAVs
Yang et al. Adaptive longitudinal control for multivehicle cooperative systems with actuator saturation under road bumps
Zhao et al. Hybrid gradient vector fields for path-following guidance
Mu et al. Path following control strategy for underactuated unmanned surface vehicle subject to multiple constraints
CN107450313A (zh) 基于自适应控制的无人机自动驾驶仪控制系统

Legal Events

Date Code Title Description
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB03 Change of inventor or designer information
CB03 Change of inventor or designer information

Inventor after: Zhao Yanjie

Inventor after: Liang Yueqian

Inventor before: Zhao Yanjie

Inventor before: Liang Yueqian

Inventor before: Yang Yi

Inventor before: Zhang Shangbin

Inventor before: Li Zhifei

Inventor before: Wang Xiangfeng

GR01 Patent grant
GR01 Patent grant