CN108845588A - 一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法 - Google Patents
一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法 Download PDFInfo
- Publication number
- CN108845588A CN108845588A CN201810653448.1A CN201810653448A CN108845588A CN 108845588 A CN108845588 A CN 108845588A CN 201810653448 A CN201810653448 A CN 201810653448A CN 108845588 A CN108845588 A CN 108845588A
- Authority
- CN
- China
- Prior art keywords
- quadrotor
- path
- angle
- axis
- coordinate system
- 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 33
- 230000001133 acceleration Effects 0.000 claims abstract description 17
- 238000005096 rolling process Methods 0.000 claims abstract description 8
- 238000004364 calculation method Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 6
- 238000013461 design Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 241000208340 Araliaceae Species 0.000 description 1
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 description 1
- 235000003140 Panax quinquefolius Nutrition 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 239000003245 coal Substances 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005489 elastic deformation Effects 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 235000008434 ginseng Nutrition 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 239000011800 void material Substances 0.000 description 1
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
-
- 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/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
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)
- Navigation (AREA)
Abstract
本发明提供一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法,属于飞行器控制技术领域。本发明首先对四旋翼飞行器建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系;然后计算四旋翼飞行器跟踪轨迹所需的高度、期望的航向角、期望的俯仰角、期望的侧向加速度;四旋翼飞行器的高度控制器获取跟踪轨迹所需的高度,姿态角控制器获取期望航向角,俯仰角控制器获取期望的俯仰角,滚转角控制器获取期望的侧向加速度,最后四旋翼飞行器在高度控制器、姿态角控制器、俯仰角控制器以及滚转角控制器的控制下按照预定轨迹飞行。本发明解决了现有四旋翼飞行器跟踪控制无法保证匀速飞行、响应延时大的问题。本发明可用于四旋翼飞行器轨迹跟踪控制。
Description
技术领域
本发明涉及一种四旋翼飞行器轨迹跟踪控制方法,属于飞行器控制技术领域。
背景技术
四旋翼飞行器具有垂直起降、悬停和机动飞行的特性,特别适合室内狭小空间和城市复杂环境执行侦察和监视任务。四旋翼飞行器是一个多变量非线性系统,动力学模型较为复杂,国内外许多高校与科研机构对其进行了广泛深入的研究。
四旋翼飞行器在执行任务的过程中,需要跟踪预先规划好的飞行轨迹,现有方法是将飞行路径离散为位置跟踪点,然后针对四旋翼飞行器设计相应的位置控制器,跟踪离散后的位置点。具体步骤为:
步骤1:对四旋翼飞行器进行姿态动力学建模和位置动力学建模,对动力学模型进行解耦,并通过线性化得到四旋翼飞行器的传递函数;建立控制对象的六自由度仿真模型,包括姿态动力学建模、位置动力学建模、执行机构建模以及模型的线性化,四旋翼无人机参考系示意图如图1所示。在对四旋翼无人机的动力学建模时,需要进行以下两条假设:
(1)将四旋翼无人机看作是刚体,认为其不发生弹性形变,并且重心位置不变,质量也不变。
(2)四旋翼一般飞行高度为相对地面几十米的距离,因此可以忽略地球曲率,以及地球的自转和公转的因素,可以将所在地面视作平面。
步骤2:分别设计姿态环串级PID控制器和位置环串级PID控制器;以姿态环为例形式如图2所示,串级PID内外两环并联调节,这样的好处的是增加系统的稳定性,抗干扰。串级PID控制器的缺点也是显而易见的,和直接对内环进行控制相比较会延长响应时间。串级PID就是将控制系统分解为内环和外环两个单级PID控制器,它增强了系统的抗干扰性能(也就是增强稳定性),对于旋翼飞行器而言相当于抵消了角速度和速度的干扰。因为有两个控制器控制飞行器,它会比单个控制器控制更多的变量,使得飞行器的适应能力更强。在整定串级PID时的经验则是:先整定内环参数,再整定外环参数。因为内环靠近输出,效果直接。
步骤3:根据特定的飞行任务设计四旋翼飞行器的飞行路线,并将航迹按照一定规则离散化为一系列的位置指令;
步骤4:将当前位置点和位置指令输入到四旋翼飞行器位置控制器中,使四旋翼飞行器跟踪离散化后的位置指令。
上述方法首先对四旋翼飞行器进行建模,并且分别设计位置环和姿态环PID控制器,然后给四旋翼飞行器的位置控制器输入一系列航迹点,飞行器逐个点跟踪来逼近整条轨迹,但是在跟踪过程中无法保证速度为期望值。如果输入的航迹点与当前位置点相距较大,则无法保证两点之间的飞行器轨迹为一条直线,而且由于控制特性,在接近目标位置点时会减速;如果给入的航迹点与当前位置点相距较小,飞行器到达每个航迹点时,会产生停顿,速度降为零,反复的加减速会大大降低轨迹跟踪品质,加长了跟踪时间;
综上可知,现有方法的缺陷在于离散的规则不容易选取,而且无法保证四旋翼飞行器匀速飞行,加入位置控制器后响应延时也会变大,最终导致飞行品质不高。
发明内容
本发明为解决现有四旋翼飞行器跟踪控制无法保证匀速飞行、响应延时大的问题,提供了一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法。
本发明所述一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法,通过以下技术方案实现:
步骤一、对四旋翼飞行器建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系OXYZ;
步骤二、根据几何关系计算四旋翼飞行器跟踪轨迹所需的高度;
步骤三、将四旋翼飞行器位置、期望路径、当前路径投影到惯性坐标系的XOY平面内,在期望路径的投影上选择一个虚拟跟踪点,并且利用虚拟跟踪点位置坐标计算:四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角、期望的航向角;
步骤四、根据四旋翼飞行器的期望的恒定飞行速度生成四旋翼飞行器的期望的俯仰角;并结合步骤三中求得的四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角,计算期望的侧向加速度;
步骤五、四旋翼飞行器的高度控制器获取跟踪轨迹所需的高度,姿态角控制器获取期望的航向角,俯仰角控制器获取期望的俯仰角,滚转角控制器获取期望的侧向加速度,四旋翼飞行器在高度控制器、姿态角控制器、俯仰角控制器以及滚转角控制器的控制下按照预定轨迹飞行。
作为对上述技术方案的进一步阐述:
进一步的,步骤一中所述建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系的具体过程包括:
对四旋翼飞行器建立直线路径坐标系opxpypzp、圆弧路径极坐标系CρNρPρ以及惯性坐标系OXYZ,定义直线路径坐标系opxpypzp的坐标原点为直线路径起点,opxp轴指向直线路径方向,opzp轴指向和惯性坐标系OZ轴指向相同,opyp轴、opxp轴、opzp轴构成右手坐标系;从惯性坐标系OXYZ到直线路径坐标系opxpypzp的转换矩阵为Ri p:
其中,χq为当前期望的直线路径方向向量的偏航角;
圆弧路径极坐标系的Nρ轴指向地理坐标系的正北方向,圆弧路径极坐标系的Pρ轴方向为当前圆弧路径的圆心指向四旋翼飞行器的方向;惯性坐标系的X轴、Y轴、Z轴分别指向地理坐标系下的北方、东方、地心方向。
进一步的,步骤二中所述计算四旋翼飞行器跟踪轨迹所需的高度的具体过程包括:
A1、当跟踪轨迹为直线路径时:
四旋翼飞行器的位置相对直线路径的位置相对偏差ep在opxpypzp坐标系下表示为:
其中,epx、epy、epz分别表示ep在opxpypzp坐标系下的xp轴方向的分量、yp轴方向的分量、zp轴方向的分量,r为四旋翼飞行器期望位置向量,p为四旋翼飞行器当前位置向量;
将相对偏差ep投影到包含直线路径方向向量的惯性坐标系下YOZ平面内,得到相对偏差的投影s:
其中,sn、se、sd分别为惯性坐标系下s在X轴方向的分量、Y轴方向的分量、Z轴方向的分量;
再结合直线路径方向向量q=(qn,qe,qd),得到:
其中,qn、qe、qd分别为惯性坐标系下q在X轴方向的分量、Y轴方向的分量、Z轴方向的分量;
得到跟踪轨迹为直线路径时,四旋翼飞行器跟踪轨迹所需的高度h为:
其中,rd为惯性坐标系下r在Z轴方向的分量;
A2、当跟踪轨迹为圆弧路径时:
圆弧路径的圆心坐标在惯性坐标系下为c=(cn,ce,cd)T,则四旋翼飞行器跟踪轨迹所需的高度h为:
h=-cd
其中,cn、ce、cd分别表示惯性坐标系下c的X轴、Y轴、Z轴坐标。
进一步的,步骤二中所述四旋翼飞行器期望位置向量r和四旋翼飞行器当前位置向量p具体为:
其中,pn、pe、pd分别为惯性坐标系下p在X轴方向的分量、Y轴方向的分量、Z轴方向的分量,rn、re分别为惯性坐标系下r在X轴方向的分量、Y轴方向的分量。
进一步的,步骤三的具体过程包括:
将四旋翼飞行器位置、期望路径、当前路径投影到惯性坐标系的XOY平面内,在期望路径的投影上选择一个距离四旋翼飞行器L1的虚拟跟踪点T,计算虚拟跟踪点位置坐标T(xt,yt),并且利用虚拟跟踪点位置坐标T(xt,yt)计算:四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η、期望的航向角χcmd;xt、yt分别表示惯性坐标系下T的X轴坐标、Y轴坐标;
B1、当跟踪轨迹为直线路径时:
当前期望的直线路径方向向量的偏航角χq和四旋翼飞行器当前速度矢量的偏航角χ由下式得出:
其中,ve表示惯性坐标系的Y轴方向的分量,vn惯性坐标系的X轴方向的分量;
计算ep在opxpypzp坐标系下yp轴方向的分量epy:
epy=-sin(χq)·(pn-rn)+cos(χq)·(pe-re)
然后根据几何关系能够得到虚拟跟踪点T的位置坐标:
其中,表示四旋翼飞行器当前位置向量p与四旋翼飞行器期望位置向量r的向量差;
再结合当前速度矢量的偏航角χ能够得到η:
利用虚拟跟踪点位置坐标计算期望的航向角χcmd:
B2、当跟踪轨迹为圆弧路径时:
虚拟跟踪点T的位置坐标计算公式为:
其中,表示四旋翼飞行器相对圆弧路径的角位置,ρ为圆弧路径的半径,λ为圆弧方向,λ∈{-1,1},当λ=-1时表示圆弧路径是逆时针,当λ=1时表示圆弧路径为顺时针;为圆心c到虚拟跟踪点T的方向向量与圆心c到四旋翼飞行器当前位置p的方向向量的夹角;
四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η为:
利用虚拟跟踪点位置坐标计算期望的航向角χcmd:
进一步的,步骤四的具体过程包括:
将四旋翼飞行器的期望的恒定飞行速度Va*转化为期望的俯仰角θcmd;
然后结合步骤三中求得的四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η,通过以下公式计算期望的侧向加速度ascmd:
L1=2Rsinη
其中,Vg是四旋翼飞行器当前飞行速度,R是对应当前侧向加速度等效的转弯半径。
进一步的,步骤三中所述的计算具体为:
其中,d表示四旋翼飞行器当前位置到圆弧路径的圆心c的距离。
本发明最为突出的特点和显著的有益效果是:
本发明所涉及的一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法,可以快速精确的响应姿态指令,然后利用非线性制导方法将期望轨迹转化为四旋翼飞行器跟踪轨迹所需的高度和期望的直线路径方向向量的偏航角,结合期望的恒定飞行速度,最终分别转化为期望的侧向加速度、期望的航向角以及期望的俯仰角。通过非线性制导方法直接给出内环控制器指令,相比于外环指令,响应速度快,延迟降低约10%;四旋翼飞行器在飞行过程中控制速度大小不变,与传统方法相比较可以大大缩短跟踪轨迹的飞行时间,而且由于速度值不变,飞行品质较好,跟踪轨迹较为平滑。可以有效的跟踪直线和圆弧轨迹,大大提高了飞行品质。
附图说明
图1为四旋翼飞行器参考系示意图;
图2为姿态环串级PID控制器原理示意图,ωqw为期望姿态角,ωdq为当前姿态角,θqw为期望角速度,θdq为当前角速度;
图3为本发明恒定飞行速度控制方式示意图;
图4为本发明进行垂直平面的投影示意图;
图5为本发明垂直平面直线轨迹跟踪示意图;
图6为本发明直线虚拟点选取原理示意图;
图7为本发明圆弧路径跟踪示意图;
图8为本发明轨迹跟踪制导逻辑示意图。
具体实施方式
具体实施方式一:结合图3对本实施方式进行说明,本实施方式给出的一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法,具体包括以下步骤:
步骤一、对四旋翼飞行器建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系OXYZ;
步骤二、根据几何关系计算四旋翼飞行器跟踪轨迹所需的高度;
步骤三、将四旋翼飞行器位置、期望路径、当前路径投影到惯性坐标系的XOY平面内,在期望路径的投影上选择一个虚拟跟踪点,并且利用虚拟跟踪点位置坐标计算:四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角、期望的航向角;
步骤四、根据四旋翼飞行器的期望的恒定飞行速度生成四旋翼飞行器的期望的俯仰角;并结合步骤三中求得的四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角,计算期望的侧向加速度;
步骤五、四旋翼飞行器的高度控制器获取跟踪轨迹所需的高度,姿态角控制器获取期望的航向角,俯仰角控制器获取期望的俯仰角,滚转角控制器获取期望的侧向加速度,四旋翼飞行器在高度控制器、姿态角控制器、俯仰角控制器以及滚转角控制器的控制下按照预定轨迹飞行。
本实施方式分别根据直线路径和圆弧路径生成相应的四旋翼飞行器跟踪轨迹所需的高度、期望的航向角、期望的俯仰角,然后基于虚拟跟踪点获取期望的侧向加速度。为保证轨迹跟踪的准确性和快速性,结合四旋翼飞行器的控制特点,本发明选择以期望的恒定飞行速度跟踪轨迹,控制方式示意图如图3所示。
具体实施方式二:本实施方式与具体实施方式一不同的是,步骤一中所述建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系的具体过程包括:
对四旋翼飞行器建立直线路径坐标系opxpypzp、圆弧路径极坐标系CρNρPρ以及惯性坐标系OXYZ,定义直线路径坐标系opxpypzp的坐标原点为直线路径起点,其opxp轴指向直线路径方向,opzp轴指向和惯性坐标系OZ轴指向相同,opyp轴、opxp轴、opzp轴构成右手坐标系;从惯性坐标系OXYZ到直线路径坐标系opxpypzp的转换矩阵为Ri p:
其中,χq为当前期望的直线路径方向向量的偏航角,即,期望的直线路径方向向量与惯性坐标系下X轴方向的夹角;
圆弧路径极坐标系的Nρ轴指向地理坐标系的正北方向,圆弧路径极坐标系的Pρ轴方向为当前圆弧路径的圆心指向四旋翼飞行器的方向;惯性坐标系的X轴、Y轴、Z轴分别指向地理坐标系下的北方、东方、地心方向。
其他步骤及参数与具体实施方式一相同。
具体实施方式三:本实施方式与具体实施方式二不同的是,步骤二中所述计算四旋翼飞行器跟踪轨迹所需的高度的具体过程包括:
A1、当跟踪轨迹为直线路径时:
四旋翼飞行器的位置相对直线路径的位置相对偏差ep在opxpypzp坐标系下可表示为:
其中,epx、epy、epz分别表示ep在opxpypzp坐标系下的xp轴方向的分量、yp轴方向的分量、zp轴方向的分量,r为四旋翼飞行器期望位置向量,p为四旋翼飞行器当前位置向量;
为了得到所需高度h,如图4所示,将相对偏差ep投影到包含直线路径方向向量的惯性坐标系下垂直平面(YOZ平面)内,得到在惯性坐标系下的相对偏差的投影s:
其中,sn、se、sd分别为惯性坐标系下s在X轴方向的分量、Y轴方向的分量、Z轴方向的分量;
如图5所示,再结合直线路径方向向量q=(qn,qe,qd),由相似三角形定理可得:
其中,qn、qe、qd分别为惯性坐标系下q在X轴方向的分量、Y轴方向的分量、Z轴方向的分量;
得到跟踪轨迹为直线路径时,四旋翼飞行器跟踪轨迹所需的高度h为:
其中,rd为惯性坐标系下r在Z轴方向的分量;
A2、当跟踪轨迹为圆弧路径时:
圆弧路径的圆心坐标在惯性坐标系下为c=(cn,ce,cd)T,则四旋翼飞行器跟踪轨迹所需的高度h为:
h=-cd
其中,cn、ce、cd分别表示惯性坐标系下c的X轴、Y轴、Z轴坐标。
其他步骤及参数与具体实施方式二相同。
具体实施方式四:本实施方式与具体实施方式三不同的是,步骤二中所述四旋翼飞行器期望位置向量r和四旋翼飞行器当前位置向量p具体为:
其中,pn、pe、pd分别为惯性坐标系下p在X轴方向的分量、Y轴方向的分量、Z轴方向的分量,rn、re、rd分别为惯性坐标系下r在X轴方向的分量、Y轴方向的分量、Z轴方向的分量。
其他步骤及参数与具体实施方式三相同。
具体实施方式五:本实施方式与具体实施方式四不同的是,所述步骤三的具体过程包括:
将四旋翼飞行器位置、期望路径、当前路径投影到惯性坐标系的XOY平面内,虚拟跟踪点的选择规则是:在期望路径的投影上选择一个与四旋翼飞行器的距离为L1的点作为虚拟跟踪点T,根据图6所示几何关系计算虚拟跟踪点位置坐标T(xt,yt),并且利用虚拟跟踪点位置坐标T(xt,yt)计算:四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η、期望的航向角χcmd;xt、yt分别表示惯性坐标系下T的X轴坐标、Y轴坐标;
B1、当跟踪轨迹为直线路径时:
当前期望的直线路径方向向量的偏航角χq和四旋翼飞行器当前速度矢量的偏航角χ可由下式得出:
其中,ve表示惯性坐标系的Y轴方向的分量,vn惯性坐标系的X轴方向的分量;
计算四旋翼飞行器的位置相对直线路径的位置相对偏差ep在opxpypzp坐标系下yp轴方向的分量epy:
epy=-sin(χq)·(pn-rn)+cos(χq)·(pe-re)
然后根据几何关系能够得到虚拟跟踪点T的位置坐标:
其中,表示四旋翼飞行器当前位置向量p与四旋翼飞行器期望位置向量r的向量差;
再结合当前速度矢量的偏航角χ能够得到四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η:
利用虚拟跟踪点位置坐标计算期望的航向角χcmd:
B2、当跟踪轨迹为圆弧路径时:
如图7所示,虚拟跟踪点T的位置坐标计算公式为:
其中,表示四旋翼飞行器相对圆弧路径的角位置,ρ为圆弧路径的半径,λ为圆弧方向,λ∈{-1,1},当λ=-1时表示圆弧路径是逆时针,当λ=1时表示圆弧路径为顺时针;为圆心c到虚拟跟踪点T的方向向量与圆心c到四旋翼飞行器当前位置p的方向向量的夹角;
四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η为:
利用虚拟跟踪点位置坐标计算期望的航向角χcmd:
其他步骤及参数与具体实施方式四相同。
具体实施方式六:本实施方式与具体实施方式五不同的是,所述步骤四的具体过程包括:
设计一个PID控制器,将四旋翼飞行器的期望的恒定飞行速度Va *转化为期望的俯仰角θcmd;
轨迹跟踪制导逻辑示意图如图8所示。然后结合步骤三中求得的四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η,通过以下公式计算期望的侧向加速度ascmd:
L1=2R sinη
其中,Vg是四旋翼飞行器当前飞行速度,R是对应当前侧向加速度等效的转弯半径。
其他步骤及参数与具体实施方式五相同。
具体实施方式七:本实施方式与具体实施方式五或四不同的是,步骤三中所述的计算具体为:
其中,d表示四旋翼飞行器当前位置到圆弧路径的圆心c的距离。
其他步骤及参数与具体实施方式五或四相同。
本发明还可有其它多种实施例,在不背离本发明精神及其实质的情况下,本领域技术人员当可根据本发明作出各种相应的改变和变形,但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。
Claims (7)
1.一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法,其特征在于,方法具体包括以下步骤:
步骤一、对四旋翼飞行器建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系OXYZ;
步骤二、根据几何关系计算四旋翼飞行器跟踪轨迹所需的高度;
步骤三、将四旋翼飞行器位置、期望路径、当前路径投影到惯性坐标系的XOY平面内,在期望路径的投影上选择一个虚拟跟踪点,并且利用虚拟跟踪点位置坐标计算:四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角、期望的航向角;
步骤四、根据四旋翼飞行器的期望的恒定飞行速度生成四旋翼飞行器的期望的俯仰角;并结合步骤三中求得的四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角,计算期望的侧向加速度;
步骤五、四旋翼飞行器的高度控制器获取跟踪轨迹所需的高度,姿态角控制器获取期望的航向角,俯仰角控制器获取期望的俯仰角,滚转角控制器获取期望的侧向加速度,四旋翼飞行器在高度控制器、姿态角控制器、俯仰角控制器以及滚转角控制器的控制下按照预定轨迹飞行。
2.根据权利要求1所述一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法,其特征在于,步骤一中所述建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系的具体过程包括:
对四旋翼飞行器建立直线路径坐标系opxpypzp、圆弧路径极坐标系CρNρPρ以及惯性坐标系OXYZ,定义直线路径坐标系opxpypzp的坐标原点为直线路径起点,opxp轴指向直线路径方向,opzp轴指向和惯性坐标系OZ轴指向相同,opyp轴、opxp轴、opzp轴构成右手坐标系;从惯性坐标系OXYZ到直线路径坐标系opxpypzp的转换矩阵为Ri p:
其中,χq为当前期望的直线路径方向向量的偏航角;
圆弧路径极坐标系的Nρ轴指向地理坐标系的正北方向,圆弧路径极坐标系的Pρ轴方向为当前圆弧路径的圆心指向四旋翼飞行器的方向;惯性坐标系的X轴、Y轴、Z轴分别指向地理坐标系下的北方、东方、地心方向。
3.根据权利要求2所述一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法,其特征在于,步骤二中所述计算四旋翼飞行器跟踪轨迹所需的高度的具体过程包括:
A1、当跟踪轨迹为直线路径时:
四旋翼飞行器的位置相对直线路径的位置相对偏差ep在opxpypzp坐标系下表示为:
其中,epx、epy、epz分别表示ep在opxpypzp坐标系下的xp轴方向的分量、yp轴方向的分量、zp轴方向的分量,r为四旋翼飞行器期望位置向量,p为四旋翼飞行器当前位置向量;
将相对偏差ep投影到包含直线路径方向向量的惯性坐标系下YOZ平面内,得到相对偏差的投影s:
其中,sn、se、sd分别为惯性坐标系下s在X轴方向的分量、Y轴方向的分量、Z轴方向的分量;
再结合直线路径方向向量q=(qn,qe,qd),得到:
其中,qn、qe、qd分别为惯性坐标系下q在X轴方向的分量、Y轴方向的分量、Z轴方向的分量;
得到跟踪轨迹为直线路径时,四旋翼飞行器跟踪轨迹所需的高度h为:
其中,rd为惯性坐标系下r在Z轴方向的分量;
A2、当跟踪轨迹为圆弧路径时:
圆弧路径的圆心坐标在惯性坐标系下为c=(cn,ce,cd)T,则四旋翼飞行器跟踪轨迹所需的高度h为:
h=-cd
其中,cn、ce、cd分别表示惯性坐标系下c的X轴、Y轴、Z轴坐标。
4.根据权利要求3所述一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法,其特征在于,步骤二中所述四旋翼飞行器期望位置向量r和四旋翼飞行器当前位置向量p具体为:
其中,pn、pe、pd分别为惯性坐标系下p在X轴方向的分量、Y轴方向的分量、Z轴方向的分量,rn、re分别为惯性坐标系下r在X轴方向的分量、Y轴方向的分量。
5.根据权利要求4所述一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法,其特征在于,所述步骤三的具体过程包括:
将四旋翼飞行器位置、期望路径、当前路径投影到惯性坐标系的XOY平面内,在期望路径的投影上选择一个距离四旋翼飞行器L1的虚拟跟踪点T,计算虚拟跟踪点位置坐标T(xt,yt),并且利用虚拟跟踪点位置坐标T(xt,yt)计算:四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η、期望的航向角χcmd;xt、yt分别表示惯性坐标系下T的X轴坐标、Y轴坐标;
B1、当跟踪轨迹为直线路径时:
当前期望的直线路径方向向量的偏航角χq和四旋翼飞行器当前速度矢量的偏航角χ由下式得出:
其中,ve表示惯性坐标系的Y轴方向的分量,vn惯性坐标系的X轴方向的分量;
计算ep在opxpypzp坐标系下yp轴方向的分量epy:
epy=-sin(χq)·(pn-rn)+cos(χq)·(pe-re)
然后根据几何关系能够得到虚拟跟踪点T的位置坐标:
其中,表示四旋翼飞行器当前位置向量p与四旋翼飞行器期望位置向量r的向量差;
再结合当前速度矢量的偏航角χ能够得到η:
利用虚拟跟踪点位置坐标计算期望的航向角χcmd:
B2、当跟踪轨迹为圆弧路径时:
虚拟跟踪点T的位置坐标计算公式为:
其中,表示四旋翼飞行器相对圆弧路径的角位置,ρ为圆弧路径的半径,λ为圆弧方向,λ∈{-1,1},当λ=-1时表示圆弧路径是逆时针,当λ=1时表示圆弧路径为顺时针;为圆心c到虚拟跟踪点T的方向向量与圆心c到四旋翼飞行器当前位置p的方向向量的夹角;
四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η为:
利用虚拟跟踪点位置坐标计算期望的航向角χcmd:
6.根据权利要求5所述一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法,其特征在于,所述步骤四的具体过程包括:
将四旋翼飞行器的期望的恒定飞行速度Va *转化为期望的俯仰角θcmd;
然后结合步骤三中求得的四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η,通过以下公式计算期望的侧向加速度ascmd:
L1=2R sinη
其中,Vg是四旋翼飞行器当前飞行速度,R是对应当前侧向加速度等效的转弯半径。
7.根据权利要求5或6所述一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法,其特征在于,步骤三中所述的计算具体为:
其中,d表示四旋翼飞行器当前位置到圆弧路径的圆心c的距离。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810653448.1A CN108845588B (zh) | 2018-06-22 | 2018-06-22 | 一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810653448.1A CN108845588B (zh) | 2018-06-22 | 2018-06-22 | 一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108845588A true CN108845588A (zh) | 2018-11-20 |
CN108845588B CN108845588B (zh) | 2021-05-07 |
Family
ID=64203092
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810653448.1A Active CN108845588B (zh) | 2018-06-22 | 2018-06-22 | 一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108845588B (zh) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109857146A (zh) * | 2019-01-07 | 2019-06-07 | 吉林大学 | 一种基于前馈和权值分配的分层无人机跟踪控制方法 |
CN109947123A (zh) * | 2019-02-27 | 2019-06-28 | 南京航空航天大学 | 一种基于视线导引律的无人机路径跟踪与自主避障方法 |
CN110888447A (zh) * | 2019-12-23 | 2020-03-17 | 西安电子科技大学 | 基于非线性pd双闭环控制的四旋翼3d路径跟踪方法 |
CN112026750A (zh) * | 2020-09-08 | 2020-12-04 | 中国人民解放军海军工程大学 | 一种基于位置误差的无人飞行器滑模控制侧滑转弯方法 |
CN112162570A (zh) * | 2020-10-10 | 2021-01-01 | 中国人民解放军海军航空大学 | 一种四旋翼直升飞机小范围动态跟踪的方法 |
CN113238582A (zh) * | 2021-07-09 | 2021-08-10 | 北京信息科技大学 | 飞行器的飞行轨迹的规划方法、装置及系统 |
CN113342044A (zh) * | 2021-06-18 | 2021-09-03 | 北京理工大学 | 一种可重复使用运载器末端能量管理段地面轨迹设计方法 |
CN114275156A (zh) * | 2021-12-31 | 2022-04-05 | 哈尔滨工业大学 | 一种基于涵道风扇的推力矢量无人飞行器 |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112286215B (zh) * | 2020-10-22 | 2021-12-21 | 福州大学 | 一种自主气敏检测飞行器的控制方法 |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101354254A (zh) * | 2008-09-08 | 2009-01-28 | 北京航空航天大学 | 一种飞行器航向跟踪方法 |
CN102419596A (zh) * | 2011-11-20 | 2012-04-18 | 北京航空航天大学 | 一种基于矢量域的小型无人机风场抗扰自适应控制方法 |
CN103576555A (zh) * | 2013-11-14 | 2014-02-12 | 哈尔滨工程大学 | 一种动力定位船舶循迹导引控制方法 |
CN104061934A (zh) * | 2014-06-10 | 2014-09-24 | 哈尔滨工业大学 | 基于惯性传感器的行人室内位置跟踪方法 |
CN104199303A (zh) * | 2014-09-18 | 2014-12-10 | 北京航空航天大学 | 一种基于矢量场制导的平流层卫星平面路径跟踪控制方法 |
US9146561B2 (en) * | 2013-12-03 | 2015-09-29 | King Fahd University Of Petroleum And Minerals | Robotic leader-follower navigation and fleet management control method |
CN105045284A (zh) * | 2015-09-21 | 2015-11-11 | 北京天航华创科技股份有限公司 | 一种抗干扰无人飞行器路径跟踪控制方法 |
CN105676641A (zh) * | 2016-01-25 | 2016-06-15 | 南京航空航天大学 | 针对四旋翼无人机非线性模型的基于反步和滑模控制技术的非线性鲁棒控制器的设计方法 |
CN106406359A (zh) * | 2016-08-30 | 2017-02-15 | 南京航空航天大学 | 一种基于虚拟目标的固定翼无人机跟踪地面目标制导方法 |
CN106444822A (zh) * | 2016-03-30 | 2017-02-22 | 北京航空航天大学 | 一种基于空间矢量场制导的平流层飞艇路径跟踪控制方法 |
CN106774373A (zh) * | 2017-01-12 | 2017-05-31 | 哈尔滨工业大学 | 一种四旋翼无人机有限时间姿态跟踪控制方法 |
-
2018
- 2018-06-22 CN CN201810653448.1A patent/CN108845588B/zh active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101354254A (zh) * | 2008-09-08 | 2009-01-28 | 北京航空航天大学 | 一种飞行器航向跟踪方法 |
CN102419596A (zh) * | 2011-11-20 | 2012-04-18 | 北京航空航天大学 | 一种基于矢量域的小型无人机风场抗扰自适应控制方法 |
CN103576555A (zh) * | 2013-11-14 | 2014-02-12 | 哈尔滨工程大学 | 一种动力定位船舶循迹导引控制方法 |
US9146561B2 (en) * | 2013-12-03 | 2015-09-29 | King Fahd University Of Petroleum And Minerals | Robotic leader-follower navigation and fleet management control method |
CN104061934A (zh) * | 2014-06-10 | 2014-09-24 | 哈尔滨工业大学 | 基于惯性传感器的行人室内位置跟踪方法 |
CN104199303A (zh) * | 2014-09-18 | 2014-12-10 | 北京航空航天大学 | 一种基于矢量场制导的平流层卫星平面路径跟踪控制方法 |
CN105045284A (zh) * | 2015-09-21 | 2015-11-11 | 北京天航华创科技股份有限公司 | 一种抗干扰无人飞行器路径跟踪控制方法 |
CN105676641A (zh) * | 2016-01-25 | 2016-06-15 | 南京航空航天大学 | 针对四旋翼无人机非线性模型的基于反步和滑模控制技术的非线性鲁棒控制器的设计方法 |
CN106444822A (zh) * | 2016-03-30 | 2017-02-22 | 北京航空航天大学 | 一种基于空间矢量场制导的平流层飞艇路径跟踪控制方法 |
CN106406359A (zh) * | 2016-08-30 | 2017-02-15 | 南京航空航天大学 | 一种基于虚拟目标的固定翼无人机跟踪地面目标制导方法 |
CN106774373A (zh) * | 2017-01-12 | 2017-05-31 | 哈尔滨工业大学 | 一种四旋翼无人机有限时间姿态跟踪控制方法 |
Non-Patent Citations (5)
Title |
---|
HUAMAN,A: "Design of a Fuzzy Sliding Mode Controller for the Autonomous Path-Following of a Quadrotor", 《IEEE LATIN AMERICA TRANSACTIONS》 * |
XIAOYU SHI: "Design of Trajectory Tracking Controller with Backstepping Method for Quadrotor Unmanned Aerial Vehicles", 《CCDC》 * |
李朝旭 等: "一种多UAV虚拟队形制导协同运动控制方法", 《火力与指挥控制》 * |
王平: "考虑时间因素的敏捷自主航天器四维姿态运动规划方法研究", 《宇航学报》 * |
陈子昂 等: "无人机导航路径跟踪精确制导仿真研究", 《计算机仿真》 * |
Cited By (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109857146A (zh) * | 2019-01-07 | 2019-06-07 | 吉林大学 | 一种基于前馈和权值分配的分层无人机跟踪控制方法 |
CN109857146B (zh) * | 2019-01-07 | 2021-10-15 | 吉林大学 | 一种基于前馈和权值分配的分层无人机跟踪控制方法 |
CN109947123A (zh) * | 2019-02-27 | 2019-06-28 | 南京航空航天大学 | 一种基于视线导引律的无人机路径跟踪与自主避障方法 |
CN110888447A (zh) * | 2019-12-23 | 2020-03-17 | 西安电子科技大学 | 基于非线性pd双闭环控制的四旋翼3d路径跟踪方法 |
CN110888447B (zh) * | 2019-12-23 | 2021-09-03 | 西安电子科技大学 | 基于非线性pd双闭环控制的四旋翼3d路径跟踪方法 |
CN112026750A (zh) * | 2020-09-08 | 2020-12-04 | 中国人民解放军海军工程大学 | 一种基于位置误差的无人飞行器滑模控制侧滑转弯方法 |
CN112162570A (zh) * | 2020-10-10 | 2021-01-01 | 中国人民解放军海军航空大学 | 一种四旋翼直升飞机小范围动态跟踪的方法 |
CN112162570B (zh) * | 2020-10-10 | 2022-12-06 | 中国人民解放军海军航空大学 | 一种四旋翼直升飞机小范围动态跟踪的方法 |
CN113342044A (zh) * | 2021-06-18 | 2021-09-03 | 北京理工大学 | 一种可重复使用运载器末端能量管理段地面轨迹设计方法 |
CN113238582A (zh) * | 2021-07-09 | 2021-08-10 | 北京信息科技大学 | 飞行器的飞行轨迹的规划方法、装置及系统 |
CN114275156A (zh) * | 2021-12-31 | 2022-04-05 | 哈尔滨工业大学 | 一种基于涵道风扇的推力矢量无人飞行器 |
Also Published As
Publication number | Publication date |
---|---|
CN108845588B (zh) | 2021-05-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108845588A (zh) | 一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法 | |
CN107966156B (zh) | 一种适用于运载火箭垂直回收段的制导律设计方法 | |
Ferrin et al. | Differential flatness based control of a rotorcraft for aggressive maneuvers | |
CN207374643U (zh) | 一种基于自主循迹取物运输系统的四旋翼飞行器 | |
CN109708639B (zh) | 飞行器平飞跟踪直线和圆弧路径的侧向制导指令生成方法 | |
CN108803639A (zh) | 一种基于反步法的四旋翼飞行器飞行控制方法 | |
CN106444822B (zh) | 一种基于空间矢量场制导的平流层飞艇路径跟踪控制方法 | |
CN105116914B (zh) | 一种平流层飞艇解析模型预测路径跟踪控制方法 | |
CN105425812B (zh) | 一种基于双模型下的无人机自动着舰轨迹控制方法 | |
CN106777489A (zh) | 无人机载光电稳定转台跟踪态建模仿真方法 | |
CN112198885B (zh) | 一种满足机动平台自主降落需求的无人机控制方法 | |
CN109613928B (zh) | 一种用于多矢量螺旋桨组合浮空器的复合控制系统及方法 | |
CN109407688A (zh) | 一种垂直起降火箭在线轨迹规划的质心运动解耦方法 | |
Gonçalves et al. | Homography-based visual servoing of an aircraft for automatic approach and landing | |
CN109857130A (zh) | 一种基于误差四元数的导弹双回路姿态控制方法 | |
CN107063244A (zh) | 一种飞行器飞行过程模拟方法 | |
CN112061424B (zh) | 一种基于融合目标姿态的机动过程能源角动态跟踪方法 | |
CN109343551A (zh) | 一种旋翼机协调转弯控制方法及系统 | |
CN109582039A (zh) | 一种采用相对导航信息的j2摄动下最优队形重构方法 | |
CN115097856A (zh) | 一种基于导航向量场的四旋翼无人机目标跟踪动态反馈控制方法 | |
CN106200664B (zh) | 一种适应长时间失控的姿态控制方法 | |
CN107678442B (zh) | 一种基于双模型下的四旋翼自主着船控制方法 | |
CN108427429B (zh) | 一种考虑动态指向约束的航天器视轴机动控制方法 | |
CN107102547A (zh) | 一种基于滑模控制理论的rlv着陆段制导律获取方法 | |
CN117008626A (zh) | 无动力伞翼无人机在风场中的精准航线跟踪引导控制方法 |
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 | ||
CB03 | Change of inventor or designer information |
Inventor after: Guo Jifeng Inventor after: Bai Chengchao Inventor after: Zheng Hongxing Inventor after: Yu Jin Inventor before: Yu Jin Inventor before: Guo Jifeng Inventor before: Bai Chengchao Inventor before: Zheng Hongxing |
|
CB03 | Change of inventor or designer information | ||
GR01 | Patent grant | ||
GR01 | Patent grant |