CN114594783A - 基于全过程约束的四旋翼实时轨迹规划及降落控制方法 - Google Patents
基于全过程约束的四旋翼实时轨迹规划及降落控制方法 Download PDFInfo
- Publication number
- CN114594783A CN114594783A CN202111575635.0A CN202111575635A CN114594783A CN 114594783 A CN114594783 A CN 114594783A CN 202111575635 A CN202111575635 A CN 202111575635A CN 114594783 A CN114594783 A CN 114594783A
- Authority
- CN
- China
- Prior art keywords
- landing
- time
- aircraft
- rotor
- rotor aircraft
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 79
- 238000005070 sampling Methods 0.000 claims abstract description 57
- 239000013598 vector Substances 0.000 claims abstract description 37
- 238000004891 communication Methods 0.000 claims abstract description 9
- 230000001133 acceleration Effects 0.000 claims description 20
- 238000005457 optimization Methods 0.000 claims description 14
- 238000012545 processing Methods 0.000 claims description 9
- 239000011159 matrix material Substances 0.000 claims description 7
- 206010048669 Terminal state Diseases 0.000 claims description 4
- 238000005096 rolling process Methods 0.000 claims description 4
- 239000000470 constituent Substances 0.000 claims description 3
- 239000000203 mixture Substances 0.000 claims description 3
- 239000013589 supplement Substances 0.000 claims description 2
- YCKRFDGAMUMZLT-UHFFFAOYSA-N Fluorine atom Chemical compound [F] YCKRFDGAMUMZLT-UHFFFAOYSA-N 0.000 claims 1
- 229910052731 fluorine Inorganic materials 0.000 claims 1
- 239000011737 fluorine Substances 0.000 claims 1
- 238000004422 calculation algorithm Methods 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 5
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
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
-
- 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
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
技术领域
本发明涉及四旋翼飞行器制导控制领域,尤其涉及飞行器 的降落追踪控制,具体涉及一种基基于全过程约束的四旋翼实 时轨迹规划及降落控制方法。
背景技术
四旋翼飞行器由于其灵活性和快速部署在工业中越来越 受欢迎,并在诸如拓扑和农业航空摄影、搜索救援行动和测绘 等应用中显示了其实用性。但由于四旋翼飞行器的有效载荷和 飞行时间是有限的,难以满足城市快递、森林救援和海上探测 等任务的需求,所以一些研究人员设计出飞行器与移动平台协 同进行任务的方案,以最大化任务能力。飞行器任务过程中, 首先搭载部分任务载荷从移动平台起飞,完成该阶段任务后, 再返回移动平台进行能源补充,并搭载下一部分任务载荷。其 中,依据任务目标,移动平台可以是卡车、船舰和飞机等海陆 空平台。
为了在规定时间内有效完成任务,需要四旋翼在整个任务 过程中实现稳定可靠飞行,尤其降落过程对四旋翼的控制能力 提出了极高的要求。因为降落平台在快速移动,而且环境非常 复杂,比如高楼林立的城市和树木丛生的森林,所以平台状态 没有规则地快速变化,导致四旋翼在降落过程中发生碰撞和倾 覆等事故。
此外,移动平台在不同环境下的运动状态不同,在城市道 路中比较平稳;在树林中,随地形的变化,车辆会产生不同程 度的倾斜;在海面上,舰艇平台会随着水面波动而上下起伏。 由于四旋翼的最终目标是降落在移动平台上,所以四旋翼的终 端状态约束需要匹配移动平台的运动状态。另外,由于移动平 台在持续移动,需要依据平台状态的变化,实时重规划出新的 降落轨迹,以满足全过程约束。而常规路径规划算法在考虑全 过程约束的同时,难以满足实时性要求,无法在任务过程中实 现轨迹重规划。
现有技术中的降落跟踪算法中,还需要降落平台在移动过 程中考虑降落飞行器的具体工况,适当减速或者保持匀速运 动,尽量保持平稳等等,即降落平台的正常工作状态受到影响, 需要以接收旋翼飞行器降落作为主要任务,使其正常的工作状 态受到一定的影响。
由于上述原因,本发明人对现有的飞行器降落控制方法做 了深入研究,以期待设计出一种能够解决上述问题的新的飞行 器降落控制方法。
发明内容
为了克服上述问题,本发明人进行了锐意研究,设计出一 种基于全过程约束的四旋翼实时轨迹规划及降落控制方法,该 方法中,通过通信系统获得降落平台的运动信息,即速度位置 和加速度等信息,通过其携带的传感器获得其自身的运动信 息,即可获得一组相对距离和相对速度信息,该信息相对来说 误差较大,适用于距离降落平台较远的情况,当距离较近时, 通过视觉传感器直接获得相对距离和相对速度信息,其精度较 高;进而获得估计的降落时间,基于预测时域,将所述降落时 间分为固定份数,如20份,每份之间的临界点即为采样点,每 份时间长度即为采样时间长度,估计出飞行器在每个采用点处的状态向量,并据此控制飞行器沿着估计轨迹飞行,到达第一 个采样点后,重复上述步骤,重新估计轨迹,再次控制飞行器 到达第一个采样点;直至飞行器到达降落平台,完成飞行器降 落,从而完成本发明。
具体来说,本发明的目的在于提供以一种基于全过程约束 的四旋翼实时轨迹规划及降落控制方法,该方法包括如下步骤:
步骤一:获得四旋翼飞行器与降落平台的相对距离Pr和相 对速度信息Vr;
步骤二:估计降落所需的时间,基于预测时域N获得采样 时间长度;
步骤三:确定四旋翼飞行器当前的行为模式,规划参考轨 迹;
步骤四:通过控制器控制四旋翼飞行器跟踪参考轨迹;
步骤五:到达第一个采样点时,重复步骤1,并判断四旋翼 飞行器与降落平台的相对距离Pr是否小于或等于预定阈值,若 不小于或等于预定阈值,则重复步骤二、步骤三、步骤四、步 骤五,直至四旋翼飞行器与降落平台的相对距离Pr小于或等于 预定阈值,四旋翼飞行器停机,落到降落平台上。
其中,在步骤一中,通过通信系统接收降落平台传递出的 降落平台的位置和速度信息,通过四旋翼飞行器上搭载的传感 器获得四旋翼飞行器的位置和速度信息,从而获得四旋翼飞行 器与降落平台的相对距离Pr和相对速度信息Vr;
或者,通过四旋翼飞行器上搭载的视觉传感器捕获到降落 平台,直接获得四旋翼飞行器与降落平台的相对距离Pr和相对 速度信息Vr。
其中,在步骤二中,降落所需的时间TA通过下式(1)获得:
其中,Pr表示相对距离矢量,||Pr||表示相对距离幅值,即 无人机到目标的相对距离长度;
er表示相对距离矢量的方向,即从无人机指向目标的单位 矢量,通过下式(1.1)获得:
其中,在步骤二中,将所述降落所需的时间TA平均分为N 段,每TA/N时间即为一个采样时间长度。
其中,当所述四旋翼飞行器通过通信系统和传感器获得四 旋翼飞行器与降落平台的相对距离Pr和相对速度信息Vr时,四 旋翼飞行器当前的行为模式为搜索模式;
当所述四旋翼飞行器通过视觉传感器获得四旋翼飞行器与 降落平台的相对距离Pr和相对速度信息Vr时,四旋翼飞行器当 前的行为模式为着陆模式。
其中,所述参考轨迹包括N个状态向量,每个状态向量都包 括坐标位置、飞行器到达该坐标位置时的速度和飞行器到达该 坐标位置时的加速度。
其中,所述规划参考轨迹的过程包括,使得目标函数最小,
当行为模式为搜索模式时,所述目标函数为下式(2),
当行为模式为着陆模式时,所述目标函数为下式(3),
xL表示四旋翼飞行器与降落平台的相对距离Pr和相对速度Vr组成的矩阵,即xL=[Pr,Vr]T;表示xL的转置;xL(i)表示在 第i个采样点的四旋翼飞行器与降落平台的相对距离Pr和相对 速度Vr组成的矩阵;
u(i)表示第i个采样点的控制量,uT(i)表示u(i)的转置;
QL、RL、WL都表示系数矩阵。
其中,在所述规划参考轨迹的过程包括,基于下式(4.1)、 (4.2)和(4.3)使得目标函数最小,
x(k+1)=Ax(k)+Bu(k) (4.1)
x(0)=x0,x(N)=xf,z(k+1)>hp(4.2)
|ap(k+1)|≤ap,max,|up(k)|≤up,max,p=x,y,z (4.3)
其中,x(k+1)表示预测获得的第k+1时刻的状态向量,即 第k+1个采样点状态向量;k=0,1,...,N-1;
x(k)表示第k时刻的状态向量,即第k个采样点状态向量;
A和B都表示系数矩阵;
u(k)表示第k个采样点的控制量;
x(0)表示当前优化时域中,初始时刻的无人机状态,x0表示 当前时刻的无人机真实状态;x(N)表示当前优化时域中,终端 时刻的无人机状态,xf表示该优化时域中,终端时刻无人机所 需满足的终端状态约束;z(k+1)表示k+1时刻的无人机高度,hp表示降落平台的高度;
|ap(k+1)|表示第k+1个采样点的加速度,|up(k)|表示第k个 采样点的控制量;ap,max表示飞行器能够提供的最大加速度,up,max表示飞行器能够提供的最大控制量。
其中,在步骤四中,在到达第一个采样点前,当滚动优化 的采样周期TA比控制器的控制周期小0.05s时,对第一个采样点 以前的轨迹做差值处理,并根据差值处理结果控制四旋翼飞行 器飞行。
其中,在步骤四中,通过下式(5)对轨迹做差值处理:
其中,x(t)表示t时刻无人机的状态,x(k)表示k时刻的无 人机状态,x(k+1)表示k+1时刻无人机的状态,,t(k)表示离散 化时间k所对应的连续时间t(k),t(k+1)表示离散化时间k+1 所对应的连续时间t(k+1),t表示连续时间中的t时刻。
本发明所具有的有益效果包括:
(1)根据本发明提供的基于全过程约束的四旋翼实时轨 迹规划及降落控制方法能够在降落平台正常机动的情况下降 落到该平台上,无需平台有针对性地等待或者匀速机动;
(2)根据本发明提供的基于全过程约束的四旋翼实时轨 迹规划及降落控制方法能够在快速机动的情况下,持续追踪移 动平台,在接近平台时通过视觉传感器自动捕获平台,整体降 落过程可实现自动化;
(3)根据本发明提供的基于全过程约束的四旋翼实时轨 迹规划及降落控制方法,把模型预测控制方法应用在轨迹规划 任务中,通过设置合理的预测时域,在兼顾追踪准确性的情况 下降低运算量,可以满足轨迹重规划的实时性要求,实现四旋 翼快速准确平稳地降落在移动平台上;
(4)根据本发明提供的基于全过程约束的四旋翼实时轨 迹规划及降落控制方法,增强了四旋翼和海陆空移动平台协同 任务的能力,另外,也可以在仿真环境中引入风场等外界干扰, 并把本文的开源飞控算法替换为个人研究的飞控算法,从而用 自主降落任务的完成度检验控制算法的可靠性、鲁棒性和抗干 扰能力。
附图说明
图1示出根据本发明一种优选实施方式的基于全过程约束 的四旋翼实时轨迹规划及降落控制方法整体逻辑图;
图2示出根据本发明实施例中四旋翼飞行器与降落平台的 路线轨迹。
具体实施方式
下面通过附图和实施例对本发明进一步详细说明。通过这 些说明,本发明的特点和优点将变得更为清楚明确。
在这里专用的词“示例性”意为“用作例子、实施例或说 明性”。这里作为“示例性”所说明的任何实施例不必解释为 优于或好于其它实施例。尽管在附图中示出了实施例的各种方 面,但是除非特别指出,不必按比例绘制附图。
根据本发明提供的基于全过程约束的四旋翼实时轨迹规 划及降落控制方法,如图1中所示,该方法包括如下步骤:
步骤一,获得四旋翼飞行器与降落平台的相对距离Pr和 相对速度信息Vr;
其中,当飞行器开始执行降落任务时,通过通信系统接收 降落平台传递出的降落平台的位置和速度信息,所述通信系统 为飞行器上搭载的设备,如雷达等,其能够接收移动平台实时 传递出的移动平台的位置、速度和加速度信息;在所述飞行器 上还搭载有传感器,通过传感器获得四旋翼飞行器的位置和速 度信息,所述传感器包括卫星信号接收系统和惯导系统;基于 移动平台的运动信息和飞行器的运动信息,获得四旋翼飞行器 与降落平台的相对距离Pr和相对速度信息Vr;
飞行器在接收上述信息的同时,还启动视觉传感器,通过 视觉传感器实时寻找捕捉移动平台,当视觉传感器发现并锁定 移动平台后,通过四旋翼飞行器上搭载的视觉传感器捕获到降 落平台,直接获得四旋翼飞行器与降落平台的相对距离Pr和相 对速度信息Vr。当视觉传感器捕获到目标后,仅通过视觉传感 器获得的信息执行飞行器的制导控制工作。
步骤二:估计降落所需的时间,基于预测时域N获得采 样时间长度;
优选地,所述降落所需的时间TA通过下式(1)获得:
其中,Pr表示相对距离矢量,||Pr||表示相对距离幅值, 即无人机到目标的相对距离长度;
er表示相对距离矢量的方向,即从无人机指向目标的单位 矢量,通过下式(1.1)获得:
Vr·er表示相对速度在视线上的投影,所述视线是指飞行 器与降落平台的连线。
在获得降落所需的时间后,将该降落所需的时间平均分N 段/份,其中的N为预测时域,每TA/N时间即为一个采样时间 长度,每份之间的临界点即为采样点,每份时间长度即为采样 时间长度。
本申请中,所述预测时域为固定值,该值越大则规划路径 上的采样点越多,轨迹越精确,但相应地,计算量增加,对硬 件设备要求越高,容易导致计算延迟,该值越小,则采样点越 少,轨迹越粗糙,容易丢失目标。本申请优选地将所述预测时 域设置为20;本发明人发现当该预测时域设置为20时四旋翼飞 行器能够准确地追踪到降落平台,并且运算量适中,能够满足 实时控制解算的需求。
步骤三:确定四旋翼飞行器当前的行为模式,规划参考轨 迹;
优选地,当所述四旋翼飞行器通过通信系统和传感器获得 四旋翼飞行器与降落平台的相对距离Pr和相对速度信息Vr时, 四旋翼飞行器当前的行为模式为搜索模式;
当所述四旋翼飞行器通过视觉传感器获得四旋翼飞行器 与降落平台的相对距离Pr和相对速度信息Vr时,四旋翼飞行器 当前的行为模式为着陆模式。
优选地,所述参考轨迹包括N个状态向量,每个状态向量 都包括坐标位置、飞行器到达该坐标位置时的速度和飞行器到 达该坐标位置时的加速度。
状态向量x=[xx,y,z,vx,vy,vz,ax,ay,az]T
xx,y,z分别表示大地坐标系中的三维位置坐标,其中,xx表 示大地坐标系中X轴方向的坐标,y表示大地坐标系中Y轴方 向的坐标,z表示大地坐标系中Z轴方向的坐标;vx,vy,vz分别 表示速度在大地坐标系中的三个方向的分量,ax,ay,az分别表 示加速度在大地坐标系中的三个方向的分量。
优选地,规划参考轨迹的过程包括,使得目标函数最小,
当行为模式为搜索模式时,所述目标函数为下式(2),
当行为模式为着陆模式时,所述目标函数为下式(3),
xL表示四旋翼飞行器与降落平台的相对距离Pr和相对速度 Vr组成的矩阵,即xL=[Pr,Vr]T;表示xL的转置;xL(i)表示在 第i个采样点的四旋翼飞行器与降落平台的相对距离Pr和相对 速度Vr组成的矩阵;
u(i)表示第i个采样点的控制量,uT(i)表示u(i)的转置;
QL、RL、WL都表示系数矩阵,其取值为
QL=[0.1,0,0,0,0,0; 0,0.1,0,0,0,0; 0,0,0.1,0,0,0; 0,0,0,0.1,0,0; 0,0,0,0,0.1,0; 0,0,0,0,0,0.1],
RL=[1,0,0; 0,1,0; 0,0,1],
WL=[0.1,0; 0,0.1]。
优选地,在所述规划参考轨迹的过程中,基于下式(4.1)、(4.2)和(4.3)的约束使得目标函数最小,
x(k+1)=Ax(k)+Bu(k) (4.1)
x(0)=x0,x(N)=xf,z(k+1)>hp (4.2)
|ap(k+1)|≤ap,max,|up(k)|≤up,max,p=x,y,z (4.3)
其中,x(k+1)表示预测获得的第k+1时刻的状态向量,即 第k+1个采样点状态向量;k=0,1,...,N-1;
x(k)表示第k时刻的状态向量,即第k个采样点状态向量;
A和B都表示系数矩阵,其取值为
A=[TA/N,0,0,TA/N,0,0,(TA/N)/2,0,0; 0,TA/N,0,0,TA/N,0,0,(TA/N)/2,0;0,0,TA/N,0,0,TA/N,0,0,(TA/N)/2; 0,0,0,TA/N,0,0,TA/N,0,0; 0,0,0,0,TA/N,0,0,TA/N,0; 0,0,0,0,0,TA/N,0,0,TA/N; 0,0,0,0,0,0,TA/N,0,0; 0,0,0,0,0,0,0,TA/N,0; 0,0,0,0,0,0,0,0,TA/N],
B=[TA/N,0,0; 0,TA/N,0; 0,0,TA/N; (TA/N)/2,0,0; 0,(TA/N)/2,0; 0,0,(TA/N)/2; (TA/N)/3,0,0; 0,(TA/N)/3,0; 0,0,(TA/N)/3]
其中,TA/N是指步骤二中计算的采样时间长度;
u(k)表示第k个采样点的控制量;
x(0)表示当前优化时域中,初始时刻的无人机状态,x0表示 当前时刻的无人机真实状态;x(N)表示当前优化时域中,终端 时刻的无人机状态,xf表示该优化时域中,终端时刻无人机所 需满足的终端状态约束;z(k+1)表示k+1时刻的无人机高度,hp表示降落平台的高度;
|ap(k+1)|表示第k+1个采样点的加速度,|up(k)|表示第k 个采样点的控制量;ap,max表示飞行器能够提供的最大加速度, up,max表示飞行器能够提供的最大控制量。
本申请中的规划参考轨迹即为解算获得N个状态向量,并 且在实际控制过程中,仅仅应用了第一个状态向量,在到达第 一个状态向量时,需要重新规划参考轨迹。
步骤四:通过控制器控制四旋翼飞行器跟踪参考轨迹;
优选地,在到达第一个采样点前,当滚动优化的采样周期 TA比控制器的控制周期小0.05s时,对第一个采样点以前的轨 迹做差值处理,并根据差值处理结果控制四旋翼飞行器飞行。 优选地,滚动优化的采样周期是指优化过程中,相邻迭代步之 间的时间间隔,控制器的控制周期是指四旋翼控制过程中,相 邻控制输入之间的时间间隔。
优选地,通过下式(5)对轨迹做差值处理:
其中,x(t)表示t时刻无人机的状态,x(k)表示k时刻的无 人机状态,x(k+1)表示k+1时刻无人机的状态,t(k)表示离散 化时间k所对应的连续时间t(k),t(k+1)表示离散化时间k+1 所对应的连续时间t(k+1),t表示连续时间中的t时刻。
通过所述差值处理能够获得更匹配控制器性能的轨迹,从 而保证控制的性能,使无人机安全平稳地跟踪轨迹,快速精准 地降落在移动平台上。
在一个优选的实施方式中,所述控制器采用开源的PX4飞 控算法对四旋翼飞行器进行控制。
步骤五:到达第一个采样点时,重复步骤一,并判断四旋 翼飞行器与降落平台的相对距离Pr是否小于或等于预定阈 值,若不小于或等于预定阈值,则重复步骤二、步骤三、步骤 四、步骤五,直至四旋翼飞行器与降落平台的相对距离Pr小 于或等于预定阈值,四旋翼飞行器停机,落到降落平台上。
其中,所述预定阈值根据实际情况选择设置,如将预定阈 值设置为降落平台直径尺寸的三分之一等。在实际工作过程 中,上述步骤五中的重复过程会持续多次,随着飞行器与降落 平台之间逐渐接近,重复的频率越来越高。本申请中,飞行器 的位置坐标是指飞行器质心的坐标,降落平台的位置坐标是指 降落平台台面的几何中心。
实施例:
降落平台的初始位置为(10,0,0)、初始速度为(3,0,0)、 初始加速度为(0,0,0),其移动轨迹如图2中的虚线所示,
四旋翼飞行器的初始位置为(0,2,3)、初始速度为 (5,0,0)、初始加速度为(0,0,0),其移动轨迹如图2中的实 现所示;
其中,四旋翼飞行器在与降落平台之间的距离缩小到2m 时,四旋翼飞行器的行为模式由搜索模式调整为着陆模式;
在四旋翼飞行器追踪降落平台的过程中,具体包括如下过 程:
步骤1,实时获得四旋翼飞行器与降落平台的相对距离Pr和相对速度信息Vr;
步骤2,通过下式(1)估计降落所需的时间,基于预测时 域N=20获得采样时间长度;
步骤3,确定四旋翼飞行器当前的行为模式,规划参考轨 迹;所述规划参考轨迹的过程包括,使得目标函数最小,
当行为模式为搜索模式时,所述目标函数为下式(2),
当行为模式为着陆模式时,所述目标函数为下式(3),
在所述规划参考轨迹的过程中,基于下式(4.1)、(4.2) 和(4.3)的约束使得目标函数最小,
x(k+1)=Ax(k)+Bu(k) (4.1)
x(0)=x0,x(N)=xf,z(k+1)>hp (4.2)
|ap(k+1)|≤ap,max,|up(k)|≤up,max,p=x,y,z (4.3)
步骤4:通过控制器控制四旋翼飞行器跟踪参考轨迹;
步骤5,到达第一个采样点时,重复步骤1,并判断四旋翼 飞行器与降落平台的相对距离Pr是否小于或等于0.5米,若不 小于或等于0.5米,则重复步骤2、步骤3、步骤4、步骤5,直 至四旋翼飞行器与降落平台的相对距离Pr小于或等于0.5米, 四旋翼飞行器停机,落到降落平台上,四旋翼飞行器降落作业 结束。
从图2可看出,四旋翼飞行器沿着实时规划出的平滑轨迹, 快速平稳精准地降落在移动平台上,降落过程总耗时4.6s。
以上结合了优选的实施方式对本发明进行了说明,不过这 些实施方式仅是范例性的,仅起到说明性的作用。在此基础上, 可以对本发明进行多种替换和改进,这些均落入本发明的保护 范围内。
Claims (10)
1.一种基于全过程约束的四旋翼实时轨迹规划及降落控制方法,其特征在于,该方法包括如下步骤:
步骤一:获得四旋翼飞行器与降落平台的相对距离Pr和相对速度信息Vr;
步骤二:估计降落所需的时间,基于预测时域N获得采样时间长度;
步骤三:确定四旋翼飞行器当前的行为模式,规划参考轨迹;
步骤四:通过控制器控制四旋翼飞行器跟踪参考轨迹;
步骤五:到达第一个采样点时,重复步骤一,并判断四旋翼飞行器与降落平台的相对距离Pr是否小于或等于预定阈值,若不小于或等于预定阈值,则重复步骤二、步骤三、步骤四、步骤五,直至四旋翼飞行器与降落平台的相对距离Pr小于或等于预定阈值,四旋翼飞行器停机,落到降落平台上。
2.根据权利要求1所述的基于全过程约束的四旋翼实时轨迹规划及降落控制方法,其特征在于,
在步骤一中,通过通信系统接收降落平台传递出的降落平台的位置和速度信息,通过四旋翼飞行器上搭载的传感器获得四旋翼飞行器的位置和速度信息,从而获得四旋翼飞行器与降落平台的相对距离Pr和相对速度信息Vr;
或者,通过四旋翼飞行器上搭载的视觉传感器捕获到降落平台,直接获得四旋翼飞行器与降落平台的相对距离Pr和相对速度信息Vr。
4.根据权利要求1所述的基于全过程约束的四旋翼实时轨迹规划及降落控制方法,其特征在于,
在步骤二中,将所述降落所需的时间TA平均分为N段,每TA/N时间即为一个采样时间长度。
5.根据权利要求1所述的基于全过程约束的四旋翼实时轨迹规划及降落控制方法,其特征在于,
当所述四旋翼飞行器通过通信系统和传感器获得四旋翼飞行器与降落平台的相对距离Pr和相对速度信息Vr时,四旋翼飞行器当前的行为模式为搜索模式;
当所述四旋翼飞行器通过视觉传感器获得四旋翼飞行器与降落平台的相对距离Pr和相对速度信息Vr时,四旋翼飞行器当前的行为模式为着陆模式。
6.根据权利要求1所述的基于全过程约束的四旋翼实时轨迹规划及降落控制方法,其特征在于,
所述参考轨迹包括N个状态向量,每个状态向量都包括坐标位置、飞行器到达该坐标位置时的速度和飞行器到达该坐标位置时的加速度。
7.根据权利要求5所述的基于全过程约束的四旋翼实时轨迹规划及降落控制方法,其特征在于,
所述规划参考轨迹的过程包括,使得目标函数最小,
当行为模式为搜索模式时,所述目标函数为下式(2),
当行为模式为着陆模式时,所述目标函数为下式(3),
xL表示四旋翼飞行器与降落平台的相对距离Pr和相对速度Vr组成的矩阵,即xL=[Pr,Vr]T;表示xL的转置;xL(i)表示在第i个采样点的四旋翼飞行器与降落平台的相对距离Pr和相对速度Vr组成的矩阵;
u(i)表示第i个采样点的控制量,uT(i)表示u(i)的转置;
QL、RL、WL都表示系数矩阵。
8.根据权利要求7所述的基于全过程约束的四旋翼实时轨迹规划及降落控制方法,其特征在于,
在所述规划参考轨迹的过程中,基于下式(4.1)、(4.2)和(4.3)的约束使得目标函数最小,
x(k+1)=Ax(k)+Bu(k) (4.1)
x(0)=x0,x(N)=xf,z(k+1)>hp (4.2)
|ap(k+1)|≤ap,max,|up(k)|≤up,max,p=x,y,z (4.3)
其中,x(k+1)表示预测获得的第k+1时刻的状态向量,即第k+1个采样点状态向量;k=0,1,...,N-1;
x(k)表示第k时刻的状态向量,即第k个采样点状态向量;
A和B都表示系数矩阵;
u(k)表示第k个采样点的控制量;
x(0)表示当前优化时域中,初始时刻的无人机状态,x0表示当前时刻的无人机真实状态;x(N)表示当前优化时域中,终端时刻的无人机状态xf表示该优化时域中,终端时刻无人机所需满足的终端状态约束;z(k+1)表示k+1时刻的无人机高度,hp表示降落平台的高度;
|ap(k+1)|表示第k+1个采样点的加速度,|up(k)|表示第k个采样点的控制量;ap,max表示飞行器能够提供的最大加速度,up,max表示飞行器能够提供的最大控制量。
9.根据权利要求1所述的基于全过程约束的四旋翼实时轨迹规划及降落控制方法,其特征在于,
在步骤四中,在到达第一个采样点前,当滚动优化的采样周期TA比控制器的控制周期小0.05s时,对第一个采样点以前的轨迹做差值处理,并根据差值处理结果控制四旋翼飞行器飞行。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111575635.0A CN114594783B (zh) | 2021-12-21 | 2021-12-21 | 基于全过程约束的四旋翼实时轨迹规划及降落控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111575635.0A CN114594783B (zh) | 2021-12-21 | 2021-12-21 | 基于全过程约束的四旋翼实时轨迹规划及降落控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114594783A true CN114594783A (zh) | 2022-06-07 |
CN114594783B CN114594783B (zh) | 2023-03-31 |
Family
ID=81803712
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111575635.0A Active CN114594783B (zh) | 2021-12-21 | 2021-12-21 | 基于全过程约束的四旋翼实时轨迹规划及降落控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114594783B (zh) |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108710383A (zh) * | 2018-05-25 | 2018-10-26 | 哈尔滨工业大学 | 一种基于航点规划与跟踪的四旋翼飞行器自主降落控制方法 |
CN109885084A (zh) * | 2019-03-08 | 2019-06-14 | 南开大学 | 一种基于单目视觉和模糊控制的多旋翼无人机自主降落方法 |
WO2019182521A1 (en) * | 2018-03-22 | 2019-09-26 | Infinium Robotics Pte Ltd | Autonomous taking off, positioning and landing of unmanned aerial vehicles (uav) on a mobile platform |
CN110989682A (zh) * | 2019-12-30 | 2020-04-10 | 福州大学 | 基于单基站的无人机精准降落方法 |
CN111338375A (zh) * | 2020-02-27 | 2020-06-26 | 中国科学院国家空间科学中心 | 基于混合策略的四旋翼无人机移动降落的控制方法及系统 |
RU2739636C1 (ru) * | 2020-07-14 | 2020-12-28 | Автономная некоммерческая организация высшего образования "Университет Иннополис" | Способ приземления БПЛА на посадочную платформу |
CN112198885A (zh) * | 2019-12-31 | 2021-01-08 | 北京理工大学 | 一种满足机动平台自主降落需求的无人机控制方法 |
CN112198894A (zh) * | 2020-07-31 | 2021-01-08 | 北京理工大学 | 旋翼无人机自主移动降落制导方法及系统 |
CN112783181A (zh) * | 2021-01-13 | 2021-05-11 | 中国人民解放军空军工程大学 | 一种基于模糊控制的多旋翼无人机集群视觉着降方法 |
-
2021
- 2021-12-21 CN CN202111575635.0A patent/CN114594783B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2019182521A1 (en) * | 2018-03-22 | 2019-09-26 | Infinium Robotics Pte Ltd | Autonomous taking off, positioning and landing of unmanned aerial vehicles (uav) on a mobile platform |
CN108710383A (zh) * | 2018-05-25 | 2018-10-26 | 哈尔滨工业大学 | 一种基于航点规划与跟踪的四旋翼飞行器自主降落控制方法 |
CN109885084A (zh) * | 2019-03-08 | 2019-06-14 | 南开大学 | 一种基于单目视觉和模糊控制的多旋翼无人机自主降落方法 |
CN110989682A (zh) * | 2019-12-30 | 2020-04-10 | 福州大学 | 基于单基站的无人机精准降落方法 |
CN112198885A (zh) * | 2019-12-31 | 2021-01-08 | 北京理工大学 | 一种满足机动平台自主降落需求的无人机控制方法 |
CN111338375A (zh) * | 2020-02-27 | 2020-06-26 | 中国科学院国家空间科学中心 | 基于混合策略的四旋翼无人机移动降落的控制方法及系统 |
RU2739636C1 (ru) * | 2020-07-14 | 2020-12-28 | Автономная некоммерческая организация высшего образования "Университет Иннополис" | Способ приземления БПЛА на посадочную платформу |
CN112198894A (zh) * | 2020-07-31 | 2021-01-08 | 北京理工大学 | 旋翼无人机自主移动降落制导方法及系统 |
CN112783181A (zh) * | 2021-01-13 | 2021-05-11 | 中国人民解放军空军工程大学 | 一种基于模糊控制的多旋翼无人机集群视觉着降方法 |
Non-Patent Citations (1)
Title |
---|
孟学斌: "基于视觉的四旋翼无人机自主降落算法研究", 《控制工程》 * |
Also Published As
Publication number | Publication date |
---|---|
CN114594783B (zh) | 2023-03-31 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Ghommam et al. | Autonomous landing of a quadrotor on a moving platform | |
Redding et al. | Vision-based target localization from a fixed-wing miniature air vehicle | |
CN109471454B (zh) | 一种指定攻击倾角的微型作业飞行器的末端制导段进入方法 | |
Cappello et al. | A low-cost and high performance navigation system for small RPAS applications | |
CN107491083B (zh) | 一种基于饱和自适应滑模控制的四旋翼自主着船方法 | |
CN103708045B (zh) | 一种探月飞船跳跃式再入的在线参数辨识方法 | |
CN112198885B (zh) | 一种满足机动平台自主降落需求的无人机控制方法 | |
Cetin et al. | Fuzzy logic based approach to design of autonomous landing system for unmanned aerial vehicles | |
Cho et al. | Autonomous ship deck landing of a quadrotor UAV using feed-forward image-based visual servoing | |
Guan et al. | Moving path following with prescribed performance and its application on automatic carrier landing | |
Acuna et al. | Vision-based UAV landing on a moving platform in GPS denied environments using motion prediction | |
Bryson et al. | Active airborne localisation and exploration in unknown environments using inertial SLAM | |
Zhu et al. | Vision/GPS-based docking control for the UAV autonomous aerial refueling | |
Symington et al. | Simulating quadrotor UAVs in outdoor scenarios | |
CN114594783B (zh) | 基于全过程约束的四旋翼实时轨迹规划及降落控制方法 | |
Jantawong et al. | Automatic landing control based on GPS for fixed-wing aircraft | |
Wu et al. | Improved EKF-SLAM algorithm of unmanned helicopter autonomous landing on ship | |
Al-Sharman et al. | Auto takeoff and precision terminal-phase landing using an experimental optical flow model for GPS/INS enhancement | |
Guan et al. | A new integrated navigation system for the indoor unmanned aerial vehicles (UAVs) based on the neural network predictive compensation | |
Pavlov et al. | Algorithm for controlling objects implementing a semi-passive guidance technique | |
Ross et al. | Stochastic real-time optimal control for bearing-only trajectory planning | |
CN112198894B (zh) | 旋翼无人机自主移动降落制导方法及系统 | |
Jordan et al. | Precision maritime localization and landing with real-time kinematic GNSS | |
Kovalev et al. | UAV's autonomous navigation principe based on Earth remote sensing data | |
Vishnevsky et al. | Modelling of uav simulator for local navigation system of tethered high-altitude platforms |
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 |