CN112241125A - 一种基于微分平坦特性的无人机轨迹跟踪方法 - Google Patents
一种基于微分平坦特性的无人机轨迹跟踪方法 Download PDFInfo
- Publication number
- CN112241125A CN112241125A CN202011183904.4A CN202011183904A CN112241125A CN 112241125 A CN112241125 A CN 112241125A CN 202011183904 A CN202011183904 A CN 202011183904A CN 112241125 A CN112241125 A CN 112241125A
- Authority
- CN
- China
- Prior art keywords
- aerial vehicle
- unmanned aerial
- motion channel
- error
- horizontal motion
- 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
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
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (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
技术领域
本发明涉及高度耦合、欠驱动、时变、非线性系统自动控制技术领域,具体涉及一种基于微分平坦特性的无人机轨迹跟踪方法。
背景技术
四旋翼无人机以其无人驾驶、高机动性、小体积、垂直起降、低制造成本等优势迅速受到军用与民用的青睐。其最大优点为无人驾驶,不需要携带与飞行员有关的各种设备,其节省出来的大量的空间和重量可以用于布置更多作业载荷,实现完成更多的飞行任务,如军事地面战场的侦察、监视、情报获取、空中打击支持,民用航拍、危险地域搜救、复杂地形勘探等。复杂的飞行任务依靠的是先进的飞行控制方法,可靠的飞行控制方法可以保证无人机在执行任务时的安全性、稳定性、敏捷性,只有这样,无人机才能准确与及时的完成飞行任务。然而目前对于同时处理、协调具有高度耦合、欠驱动、时变、非线性、带约束的四旋翼无人机系统方面,并没有比较突出的方法,传统的方法或是计算复杂度高,或是进行了诸多假设以提高求解速度,在对无人机跟踪速度要求较高的任务中,很难得到应用。
发明内容
有鉴于此,本发明提供了一种基于微分平坦特性的无人机轨迹跟踪方法,能够降低计算复杂度,提高跟踪速度,适用于对无人机跟踪速度要求较高的任务。
为达到上述目的,本发明的技术方案包括以下步骤:
步骤一、针对无人机系统进行建模,并通过设置虚拟输入将无人机系统的垂直运动与水平运动解耦。
步骤二、根据无人机系统的微分平坦性质,将无人机系统从六自由度、四输入的欠驱动系统转换为四自由度、四输入的全驱动系统,并求解出姿态内环的角度期望值。
步骤三、根据步骤一解耦后的无人机系统作为实际系统,另外设置参考系统与误差系统,针对误差系统的水平运动通道进行基于模型预测控制的轨迹跟踪;针对误差系统的垂直运动通道,基于多面体微分包含理论,实时求解终端惩罚矩阵,保证系统稳定。
进一步地,步骤一,针对无人机系统进行建模,并通过设置虚拟输入将无人机系统的垂直运动与水平运动解耦,具体为:
首先针对无人机系统构建地面坐标系和机体坐标系;
地面坐标系固定于地球表面,坐标系原点OE固定于地面上飞行器的起飞点,OEXE轴指向飞行器指定的飞行方向,OEZE轴铅垂向下,OEYE轴垂直OEXEZE平面,按右手定则确定。
机体坐标系固定在无人机的机体上,并随无人机运动,原点OB在无人机质心,OBXB轴在无人机对称平面内,平行于无人机前后旋翼的连接线,指向前;竖轴OBZB平行于左右旋翼连线,指向右为正方向;OBYB与OBXBZB平面垂直,方向按照右手定则确定。
利用无人机的三个姿态角度俯仰角θ、偏航角φ和滚转角ψ计算地面坐标系与机体坐标系的转换矩阵R:
无人机为四旋翼飞行器,四旋翼飞行器是一个刚体且对称,在运动中其质量不变;地面坐标系为惯性坐标系,视地球表面为一平面,且重力加速度不随高度变化;不计入地球公转与自转对四旋翼飞行器的影响;四旋翼飞行器的形状与质量关于其几何对称轴对称;四旋翼飞行器旋翼电机所提供的升力与转速成正比,且不计旋翼的螺旋效应;四旋翼飞行器在运动过程中偏航角ψ=0。
在机体坐标系下,四旋翼飞行器四个旋翼产生的升力为Fi(i∈{1,2,3,4}),F1~F4分别为第1~第4旋翼产生的升力,四个旋翼产生的力矩为Mi(i∈{1,2,3,4}),M1~M4分别为第1~第4旋翼产生的力矩;四旋翼飞行器升力U1、横滚矩U2、俯仰矩U3均由四个旋翼升力提供,而偏航力矩U4由旋翼产生的力矩提供;设置无人机的四个输入分别为
其中l为四旋翼飞行器的基体坐标系中旋翼中心线到OBYB的距离。
根据牛顿第二定律与动量矩定律得到如下无人机系统模型
其中m是无人机的质量;g是重力加速度;r为无人机重心在地面坐标系下的位置,为r的二阶导数;iz=[0 0 1]T为方向向量;J为转动惯量矩阵;ω=[p,q,r]为机体坐标系下无人机角速度,p、q、r分别为无人机角速度在机体坐标系下三轴分量,为ω的一阶导数;τ为控制无人机的合外力矩。
将公式(3)、(4)展开为以下解析形式无人机动力学方程为
其中(x、y、z)为四旋翼飞行器机体在地面坐标系下的位置坐标;Ix、Iy、Iz分别为四旋翼飞行器机体相对于机体坐标系三轴的转动惯量。
定义第一虚拟控制量ux和第二虚拟控制量uy;
ux=cosψsinθcosφ+sinψsinφ (6)
uy=sinψsinθcosφ-cosψsinφ (7)
通过设置第一虚拟控制量ux和第二虚拟控制量uy,将无人机系统的垂直运动与水平运动解耦,得到四旋翼飞行器的动力学空间状态方程为:
进一步地,步骤二具体为:设置微分平坦输出包括四个,分别为第一输出Z1=x,第二输出Z2=y,第三输出Z3=z,第四输出Z4=ψ。
则U1,ux,uy表示为
俯仰角θ、滚转角φ由微分平坦输出Zi,i∈{1,2,3,4}及其有限阶导数表示
解得滚转角期望值φr和俯仰角期望值θr用于姿态内环的控制,具体表达式为
φr=arcsin(ur,xsinψr-ur,ycosψr) (12)
其中ψr为偏航角期望值,其数值为0;ur,x为水平运动通道内的第一虚拟参考输入,ur,y为水平运动通道内的第二虚拟参考输入,ur,z为垂直运动通道的参考输入。
进一步地,步骤三中,针对误差系统的水平运动通道采用模型预测控制方法进行轨迹跟踪,具体为:
设置水平运动通道内的状态向量为水平运动通道内的输入量为uxy=[ux,uy]T,ux为第一虚拟控制量,uy为第二虚拟控制量;水平运动通道内的参考轨迹为xr、yr为参考轨迹中无人机机体在地面坐标系的位置坐标,分别为xr、yr的一阶导数;水平运动通道内的参考输入为ur,xy=[ur,x,ur,y]T;水平运动通道内的误差状态向量水平运动通道内的输入误差量则四旋翼飞行器水平运动通道的误差动力学方程,即水平通道误差系统具体为:
利用零阶保持器将水平运动的误差动力学方程进行离散化,对于采样点k,得到水平运动通道离散化误差状态方程:
其中Ts为离散化过程中的采样时间;为离散后的误差状态向量系数矩阵, 为离散后的输入误差量系数矩阵, 为水平运动通道内的误差状态向量的离散化后在采样点k处数值;为水平运动通道内的输入误差量的离散化后在采样点k处数值;
利用模型预测控制方法对水平通道误差系统设计控制器,其控制策略为在每个采样时刻k通过求解优化控制问题,将解得的控制量输入系统;定义如下水平运动通道的优化代价函数:
其中N为预测时域;Qxy为水平运动通道的状态权重矩阵;Rxy为水平运动通道的输入权重矩阵;Pxy为水平运动通道的终端惩罚矩阵;Xxy为水平运动通道状态集合;Uxy为水平运动通道输入集合;
Ωxy为平衡点附近的邻域,其中γxy决定终端域的大小;表示在k时刻预测k+i时刻的水平运动通道内的误差状态向量;表示在k时刻预测k+i时刻的水平运动通道内的输入误差量;表示在终端域内水平运动通道的误差状态向量;ξr,xy(k)为k时刻水平运动通道的参考轨迹;ur,xy(k)为k时刻水平运动通道内的参考输入。
进一步地,针对误差系统的垂直运动通道,基于多面体微分包含理论,实时求解终端惩罚矩阵,保证系统稳定,具体为:
设置垂直运动通道内的状态向量为垂直运动通道内的输入量为uz;垂直运动通道的参考轨迹为zr为参考轨迹中无人机机体在地面坐标系的高度坐标;垂直运动通道内的参考输入为ur,z;垂直运动通道内的误差状态向量水平运动通道内的输入误差量针对四旋翼飞行器垂直运动通道的误差动力学方程,利用零阶保持器将垂直方向误差系统进行离散化,得到垂直方向离散化误差状态方程:
其中垂直方向误差状态向量系数矩阵为垂直方向输入误差量系数矩阵为θ(k)为k时刻的俯仰角,φ(k)为k时刻的滚转角;分别为垂直方向的误差状态向量的离散化后在采样点k,k+1处数值;为垂直方向的输入误差量的离散化后在采样点k处数值。
垂直方向误差系统为时变系统,利用模型预测控制方法对误差系统设计控制器,其控制策略为在每个采样时刻k通过求解优化控制问题,将解得的控制量输入系统;通过滚动优化的方式,实现对飞行器的控制;定义如下垂直运动通道的优化代价函数:
其中N为预测时域;Qz为垂直运动通道的状态权重矩阵;Rz为垂直运动通道的输入权重矩阵;Pz为垂直运动通道的终端惩罚矩阵;Xz为垂直运动状态集合;Uz为垂直运动输入集合,Uz:={uz||uz|≤μz}。
在每个采样时刻,通过对预测域的最后一步,求解垂直运动通道的终端惩罚矩阵Pz,确保在k>N时系统稳定;
Pz按照如下方式求解:
其中tr(Pz)为矩阵Pz的迹。
LMIi(Y,W)为约束条件,Y为第一指代值,Y=Pz -1;W为第二指代值,W=KPz -1,K为线性状态反馈控制系数;Qz为垂直运动通道的状态权重矩阵;Rz为垂直运动通道的输入权重矩阵;I为单位矩阵。
在求得终端惩罚矩阵Pz后,对垂直运动通道采用与水平运动通道相同的模型预测控制方法进行轨迹跟踪。
有益效果:
本发明提出了一种基于微分平坦特性的无人机轨迹跟踪方法,通过对四旋翼飞行器进行动力学与运动学建模,并对其模型进行微分平坦属性判定,基于此性质通过非线性变化,将欠驱动四旋翼模型转化为全驱动系统。设置虚拟控制量,将高度耦合系统解耦,并将其转化为线性系统。利用模型预测控制处理有约束系统的航迹跟踪控制问题,针对系统时变特性,构造多面体描述系统包含原时变系统,减少在线优化计算复杂度,并以此推导出使系统稳定的实时终端罚值。本发明设计了针对时变系统的模型预测控制方法,处理无人机轨迹跟踪过程中受到的约束与系统时变特性,为进一步降低求解复杂度,将模型预测控制转化为二次规划问题求解。本发明能够有效的完成无人机轨迹跟踪任务,很好的处理无人机非线性特性,计算复杂度低,工程实践中易于实现,跟踪速度快,仿真效果较好。
附图说明
图1为三维空间中的仿真轨迹与目标轨迹示意图;
图2为仿真轨迹与目标轨迹示意图;
图3为仿真实验中无人机姿态角曲线示意图;
图4为本发明实施例提供的一种基于微分平坦特性的无人机轨迹跟踪方法的流程图。
具体实施方式
下面结合附图并举实施例,对本发明进行详细描述。
本发明通过对四旋翼飞行器进行动力学与运动学建模,并对其模型进行微分平坦属性判定,基于此性质通过非线性变化,将欠驱动四旋翼模型转化为全驱动系统。设置虚拟控制量,将高度耦合系统解耦,并将其转化为线性系统。利用模型预测控制处理有约束系统的航迹跟踪控制问题,针对系统时变特性,构造多面体描述系统包含原时变系统,减少在线优化计算复杂度,并以此推导出使系统稳定的实时终端罚值。
本发明提供的一种基于微分平坦与模型预测控制的无人机轨迹跟踪控制方法,其流程如图4所示,包括如下步骤:
步骤一、无人机建模、解耦处理。无人机系统具有高耦合特征,通过设置虚拟输入将无人机系统各个通道解耦,便于之后分通道进行轨迹跟踪控制。
步骤二、微分平坦特性判定与平坦输出选择。设置微分平坦输出为Z1=x,Z2=y,Z3=z,Z4=ψ。通过推导可知姿态角φ和θ、系统其他状态量x,y,z,ψ与输入U1,ux,uy,U4可以由平坦输出Zi(i=1,2,3,4)及其有限阶导数表示。通过微分平坦性质证明,可将系统从六自由度,四输入的欠驱动系统转换为四自由度,四输入的全驱动系统。同时,依据此性质求解出姿态内环的角度期望值。
步骤三、构造四旋翼轨迹跟踪问题。根据步骤一解耦后的系统,另外设置参考系统与误差系统,针对误差系统进行基于模型预测控制的轨迹跟踪。针对垂直运动通道的时变特性,基于多面体微分包含理论,实时求解终端惩罚矩阵,保证系统稳定。
本发明实施例中,步骤一具体为,首先对飞行器系统运行的坐标系进行定义,其中OE为地面坐标系,OB为机体坐标系。地面坐标系固定于地球表面,坐标系原点固定于地面上飞行器的起飞点,OEXE轴指向飞行器指定的飞行方向,OEZE轴铅垂向下,OEYE轴垂直OEXEZE平面,按右手定则确定。机体坐标系固定在机体上,并随飞行器运动,原点在飞行器质心,OBXB轴在飞行器对称平面内,平行于前后旋翼的连接线,指向前;竖轴OBZB平行于左右旋翼连线,指向右为正方向;OBYB与OBXBZB平面垂直,方向按照右手定则确定。θ,φ,ψ分别表示俯仰角、滚转角、偏航角,利用三个姿态角度可以将地面坐标系与机体坐标系联系起来,转换矩阵表达式为:
在这里做出以下假设:
1.四旋翼飞行器是一个刚体且对称,在运动中其质量不变;
2.地面坐标系为惯性坐标系,视地球表面为一平面,且重力加速度不随高度的变化而变化;
3.不计入地球公转与自转对飞行器的影响;
4.飞行器的形状与质量关于其几何对称轴对称;
5.飞行器旋翼电机所提供的升力与转速成正比F=kω2,且不计旋翼的螺旋效应;
6.飞行器在运动过程中偏航角ψ=0。
在机体坐标系下,四旋翼飞行器四个旋翼产生的升力为Fi(i∈{1,2,3,4}),F1~F4分别为第1~第4旋翼产生的升力,四个旋翼产生的力矩为Mi(i∈{1,2,3,4}),M1~M4分别为第1~第4旋翼产生的力矩;四旋翼飞行器升力U1、横滚矩U2、俯仰矩U3均由四个旋翼升力提供,而偏航力矩U4由旋翼产生的力矩提供;设置无人机的四个输入分别为:
其中l为四旋翼飞行器的基体坐标系中,旋翼中心线到OBYB的距离;
根据牛顿第二定律与动量矩定律得到如下无人机系统模型
其中m是无人机的质量;g是重力加速度;r为无人机重心在地面坐标系下的位置,为r的二阶导数;iz=[0 0 1]T为方向向量;J为转动惯量矩阵;ω=[p,q,r]为机体坐标系下无人机角速度,p、q、r分别为无人机角速度在机体坐标系下三轴分量,为ω的一阶导数;τ为控制无人机的合外力矩。
可将式(3)、(4)展开为以下解析形式飞行器动力学方程为
其中(x、y、z)为四旋翼飞行器机体在地面坐标系下的位置坐标;Ix、Iy、Iz分别为四旋翼飞行器机体相对于机体坐标系三轴的转动惯量。
定义第一虚拟控制量ux和第二虚拟控制量uy;
ux=cosψsinθcosφ+sinψsinφ (6)
uy=sinψsinθcosφ-cosψsinφ (7)
通过设置第一虚拟控制量ux和第二虚拟控制量uy,将无人机系统的垂直运动与水平运动解耦,得到四旋翼飞行器的动力学空间状态方程为:
通过设置虚拟控制量,将飞行器垂直运动与水平运动解耦,便于第三步中基于模型预测控制的轨迹跟踪计算,并减少对时变系统的在线计算复杂度。
本发明实施例中,步骤二微分平坦性质证明具体为:设置微分平坦输出包括四个,分别为第一输出Z1=x,第二输出Z2=y,第三输出Z3=z,第四输出Z4=ψ;
则U1,ux,uy表示为
俯仰角θ、滚转角φ由微分平坦输出Zi,i∈{1,2,3,4}及其有限阶导数表示
根据求得的角度表达式并带入系统方程可求解出所有用平坦输出表示的系统输入,至此飞行器的所有状态量与输入量均可由微分平坦输出表示出来,飞行器具有微分平坦特性。原有的飞行器运动学方程有6个自由度,4个控制输入,为欠驱动系统,且原有飞行器动力学方程水平运动与垂直运动高度耦合。利用飞行器微分平坦特征并设置虚拟控制量,将原系统变为4个自由度,4个控制输入的全驱动系统,并解除飞行器水平运动与垂直运动之间的耦合关系,从而简化其控制器设计。
依据此特性处理虚拟输入,解得姿态角φr和θr用于姿态内环的控制,具体表达式为
φr=arcsin(ur,xsinψr-ur,ycosψr) (12)
其中ψr为偏航角期望值,其数值为0;ur,x为水平运动通道内的第一虚拟参考输入,ur,y为水平运动通道内的第二虚拟参考输入,ur,z为垂直运动通道的参考输入。
进一步,步骤三描述的无人机在进行轨迹跟踪的过程中,会受到诸多状态约束与输入约束限制,有约束的轨迹跟踪问题,本质上就是一个最优控制问题,但其解析解非常难以求得,而模型预测控制能很好的处理这些带约束的轨迹跟踪问题。
设置水平运动通道内的状态向量为水平运动通道内的输入量为uxy=[ux,uy]T,ux为第一虚拟控制量,uy为第二虚拟控制量;水平运动通道内的参考轨迹为xr、yr为参考轨迹中无人机机体在地面坐标系的位置坐标,分别为xr、yr的一阶导数;水平运动通道内的参考输入为ur,xy=[ur,x,ur,y]T;水平运动通道内的误差状态向量水平运动通道内的输入误差量则四旋翼飞行器水平运动通道的误差动力学方程,即水平通道误差系统具体为:
利用零阶保持器将水平运动的误差动力学方程进行离散化,对于采样点k,得到水平运动通道离散化误差状态方程:
其中Ts为离散化过程中的采样时间;为离散后的误差状态向量系数矩阵, 为离散后的输入误差量系数矩阵, 为水平运动通道内的误差状态向量的离散化后在采样点k处数值;为水平运动通道内的输入误差量的离散化后在采样点k处数值。
可见,飞行器水平方向为时不变系统,可以利用模型预测控制方法对误差系统设计控制器,其控制策略为在每个采样时刻k通过求解优化控制问题,将解得的控制量输入系统。通过滚动优化的方式,实现对飞行器的控制。定义优化代价函数:
其中N为预测时域;Qxy为水平运动通道的状态权重矩阵;Rxy为水平运动通道的输入权重矩阵;Pxy为水平运动通道的终端惩罚矩阵;Xxy为水平运动状态集合;Uxy为水平运动输入集合;Ωxy为平衡点附近的邻域,其中γxy决定终端域的大小;表示在k时刻预测k+i时刻的水平运动通道内的误差状态向量;表示在k时刻预测k+i时刻的水平运动通道内的输入误差量;表示在终端域内水平运动通道的误差状态向量;ξr,xy(k)为k时刻水平运动通道的参考轨迹;ur,xy(k)为k时刻水平运动通道内的参考输入。
同理,设置垂直运动通道内的状态向量为垂直运动通道内的输入量为uz;垂直运动通道的参考轨迹为zr为参考轨迹中无人机机体在地面坐标系的高度坐标;垂直运动通道内的参考输入为ur,z;垂直运动通道内的误差状态向量水平运动通道内的输入误差量针对四旋翼飞行器垂直运动通道的误差动力学方程,利用零阶保持器将垂直方向误差系统进行离散化,得到垂直方向离散化误差状态方程:
其中垂直方向误差状态向量系数矩阵为垂直方向输入误差量系数矩阵为θ(k)为k时刻的俯仰角,φ(k)为k时刻的滚转角;分别为垂直方向的误差状态向量的离散化后在采样点k,k+1处数值;为垂直方向的输入误差量的离散化后在采样点k处数值;
即垂直方向误差系统为时变系统,利用模型预测控制方法对误差系统设计控制器,其控制策略为在每个采样时刻k通过求解优化控制问题,将解得的控制量输入系统;通过滚动优化的方式,实现对飞行器的控制;定义如下垂直运动通道的优化代价函数:
其中N为预测时域;Qz为垂直运动通道的状态权重矩阵;Rz为垂直运动通道的输入权重矩阵;Pz为垂直运动通道的终端惩罚矩阵;Xz为垂直运动状态集合;Uz为垂直运动输入集合,Uz:={uz||uz|≤μz}。
垂直方向的时变特性导致在模型预测控制中终端域与终端惩罚矩阵无法通过离线求解方式得到。本方法构造多面体描述系统包含原时变系统,保证系统状态始终保持在多面体系统构造的不变集内,以此降低在线求解的复杂度。具体为,在每个采样时刻,通过对预测域的最后一步,选取合适的加权矩阵Pz,确保在k>N时系统稳定。
引入线性微分包含理论:考虑函数y=H(x)满足H(0)=0,且y=H(x)在闭凸集Ψ内连续且偏导存在,则对于闭凸集的任意一点a∈Ψ,存在向量使得其中凸包Θ∈Co{ζ1,ζ2,…,ζL},L为凸包顶点的个数。
则式(19)可以写成
令Y=Pz -1 W=KPz -1
上面最优问题变为线性矩阵不等式问题,约束条件为
综上,对终端域的求解变为对Pz的迹的求解,即Pz按照如下方式求解:
其中tr(Pz)为矩阵Pz的迹;
LMIi(Y,W)为约束条件,Y为第一指代值,Y=Pz -1;W为第二指代值,W=KPz -1,K为线性状态反馈控制系数;Qz为垂直运动通道的状态权重矩阵;Rz为垂直运动通道的输入权重矩阵;I为单位矩阵。
在求得终端惩罚矩阵Pz后,对垂直运动通道采用与水平运动通道相同的模型预测控制方法进行轨迹跟踪。
为进一步降低求解复杂度,本方法将继续对前述模型预测控制进行进一步优化,以垂直方向误差系统为例,设置增广状态向量
上述有约束的优化问题转化为二次规划问题
由于垂直方向为时变系统,所以二次规划问题的矩阵H(k),f(k)也为时间的函数,需要在线实时求解。
下面对本发明的具体实施方式进行详细说明
步骤一,无人机建模,根据牛顿第二定律与动量矩定律可以得到如下系统模型
这里选择的参数如下:
m=1kg,J=diag[0.05,0.05,0.1],g=9.8m/s2
步骤二,微分平坦性质证明,将微分平坦输出设置为Z1=x,Z2=y,Z3=z,Z4=ψ,将系统状态量与输入量用微分平坦输出及其导数表示。
步骤三,构造四旋翼轨迹跟踪问题。无人机初始位置设置为(0,0,0)T,初始姿态设置为(0,0,0)T,设置目标为zr=5,半径R=2的圆形轨迹。设置采样时间Ts=0.05s,预测时域N=20。专利提供的方法具体为
1.轨迹生成,包括生成目标位置与目标速度。
2.计算飞行器实时位置与目标轨迹的差值,将此差值作为模型预测控制的初值,计算预测时域内的预测状态值,并将计算出的第一个输入与实时的参考系统输入相加,得到实际系统的输入U1,ux,uy,U4,参考系统输入可以应用无人机微分平坦特性计算得到。
3.通过专利中提供的公式求解出内环姿态控制器的输入θr,φr,ψr,通过内环控制器求解出U1,U2,U3,U4控制无人机运动。
4.将无人机的状态反馈至位置外环与姿态内环进行下一步计算。
通过本发明的具体实例可以看出,本发明可以很好的处理无人机系统高度耦合、欠驱动、时变、非线性的特点,根据图1、图2的轨迹跟踪结果可以看出,该方法能够在满足约束情况下,无人机的跟踪轨迹平滑、连续,跟踪速度较快。根据图3的轨迹跟踪结果可以看出无人机姿态稳定。
综上所述,以上仅为本发明的较佳实施例而已,并非用于限定本发明的保护范围。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (5)
1.一种基于微分平坦特性的无人机轨迹跟踪方法,其特征在于,包括以下步骤:
步骤一、针对无人机系统进行建模,并通过设置虚拟输入将无人机系统的垂直运动与水平运动解耦;
步骤二、根据无人机系统的微分平坦性质,将无人机系统从六自由度、四输入的欠驱动系统转换为四自由度、四输入的全驱动系统,并求解出姿态内环的角度期望值;
步骤三、根据步骤一解耦后的无人机系统作为实际系统,另外设置参考系统与误差系统,针对误差系统的水平运动通道进行基于模型预测控制的轨迹跟踪;针对误差系统的垂直运动通道,基于多面体微分包含理论,实时求解终端惩罚矩阵,保证系统稳定。
2.如权利要求1所述的方法,其特征在于,所述步骤一,针对无人机系统进行建模,并通过设置虚拟输入将无人机系统的垂直运动与水平运动解耦,具体为:
首先针对无人机系统构建地面坐标系和机体坐标系;
所述地面坐标系固定于地球表面,坐标系原点OE固定于地面上飞行器的起飞点,OEXE轴指向飞行器指定的飞行方向,OEZE轴铅垂向下,OEYE轴垂直OEXEZE平面,按右手定则确定;
机体坐标系固定在无人机的机体上,并随无人机运动,原点OB在无人机质心,OBXB轴在无人机对称平面内,平行于无人机前后旋翼的连接线,指向前;竖轴OBZB平行于左右旋翼连线,指向右为正方向;OBYB与OBXBZB平面垂直,方向按照右手定则确定;
利用无人机的三个姿态角度俯仰角θ、偏航角φ和滚转角ψ计算地面坐标系与机体坐标系的转换矩阵R:
所述无人机为四旋翼飞行器,所述四旋翼飞行器是一个刚体且对称,在运动中其质量不变;所述地面坐标系为惯性坐标系,视地球表面为一平面,且重力加速度不随高度变化;不计入地球公转与自转对所述四旋翼飞行器的影响;四旋翼飞行器的形状与质量关于其几何对称轴对称;四旋翼飞行器旋翼电机所提供的升力与转速成正比,且不计旋翼的螺旋效应;四旋翼飞行器在运动过程中偏航角ψ=0;
在机体坐标系下,所述四旋翼飞行器四个旋翼产生的升力为Fi,i∈{1,2,3,4},F1~F4分别为第1~第4旋翼产生的升力,四个旋翼产生的力矩为Mi,i∈{1,2,3,4},M1~M4分别为第1~第4旋翼产生的力矩;所述四旋翼飞行器升力U1、横滚矩U2、俯仰矩U3均由四个旋翼升力提供,而偏航力矩U4由旋翼产生的力矩提供;设置无人机的四个输入分别为
其中l为四旋翼飞行器的基体坐标系中,旋翼中心线到OBYB的距离;
根据牛顿第二定律与动量矩定律得到如下无人机系统模型
其中m是无人机的质量;g是重力加速度;r为无人机重心在地面坐标系下的位置,为r的二阶导数;iz=[0 0 1]T为方向向量;J为转动惯量矩阵;ω=[p,q,r]为机体坐标系下无人机角速度,p、q、r分别为无人机角速度在机体坐标系下三轴分量,为ω的一阶导数;τ为控制无人机的合外力矩。
将公式(3)、(4)展开为以下解析形式无人机动力学方程为
其中(x、y、z)为四旋翼飞行器机体在地面坐标系下的位置坐标;Ix、Iy、Iz分别为四旋翼飞行器机体相对于机体坐标系三轴的转动惯量;
定义第一虚拟控制量ux和第二虚拟控制量uy;
ux=cosψsinθcosφ+sinψsinφ (6)
uy=sinψsinθcosφ-cosψsinφ (7)
通过设置第一虚拟控制量ux和第二虚拟控制量uy,将无人机系统的垂直运动与水平运动解耦,得到四旋翼飞行器的动力学空间状态方程为:
4.如权利要求3所述的方法,其特征在于,所述步骤三中,所述针对误差系统的水平运动通道采用模型预测控制方法进行轨迹跟踪,具体为:
设置水平运动通道内的状态向量为水平运动通道内的输入量为uxy=[ux,uy]T,ux为第一虚拟控制量,uy为第二虚拟控制量;水平运动通道内的参考轨迹为xr、yr为参考轨迹中无人机机体在地面坐标系的位置坐标,分别为xr、yr的一阶导数;水平运动通道内的参考输入为ur,xy=[ur,x,ur,y]T;水平运动通道内的误差状态向量水平运动通道内的输入误差量则四旋翼飞行器水平运动通道的误差动力学方程,即水平通道误差系统具体为:
利用零阶保持器将所述水平运动的误差动力学方程进行离散化,对于采样点k,得到水平运动通道离散化误差状态方程:
其中Ts为离散化过程中的采样时间;为离散后的误差状态向量系数矩阵, 为离散后的输入误差量系数矩阵, 为水平运动通道内的误差状态向量的离散化后在采样点k处数值;为水平运动通道内的输入误差量的离散化后在采样点k处数值;
利用模型预测控制方法对水平通道误差系统设计控制器,其控制策略为在每个采样时刻k通过求解优化控制问题,将解得的控制量输入系统;定义如下水平运动通道的优化代价函数:
其中N为预测时域;Qxy为水平运动通道的状态权重矩阵;Rxy为水平运动通道的输入权重矩阵;Pxy为水平运动通道的终端惩罚矩阵;Xxy为水平运动通道状态集合;Uxy为水平运动通道输入集合;
5.如权利要求4所述的方法,其特征在于,所述针对误差系统的垂直运动通道,基于多面体微分包含理论,实时求解终端惩罚矩阵,保证系统稳定,具体为:
设置垂直运动通道内的状态向量为垂直运动通道内的输入量为uz;垂直运动通道的参考轨迹为zr为参考轨迹中无人机机体在地面坐标系的高度坐标;垂直运动通道内的参考输入为ur,z;垂直运动通道内的误差状态向量水平运动通道内的输入误差量针对四旋翼飞行器垂直运动通道的误差动力学方程,利用零阶保持器将垂直方向误差系统进行离散化,得到垂直方向离散化误差状态方程:
其中垂直方向误差状态向量系数矩阵为垂直方向输入误差量系数矩阵为θ(k)为k时刻的俯仰角,φ(k)为k时刻的滚转角;分别为垂直方向的误差状态向量的离散化后在采样点k,k+1处数值;为垂直方向的输入误差量的离散化后在采样点k处数值;
垂直方向误差系统为时变系统,利用模型预测控制方法对误差系统设计控制器,其控制策略为在每个采样时刻k通过求解优化控制问题,将解得的控制量输入系统;通过滚动优化的方式,实现对飞行器的控制;定义如下垂直运动通道的优化代价函数:
其中N为预测时域;Qz为垂直运动通道的状态权重矩阵;Rz为垂直运动通道的输入权重矩阵;Pz为垂直运动通道的终端惩罚矩阵;Xz为垂直运动状态集合;Uz为垂直运动输入集合,Uz:={uz||uz|≤μz};
在每个采样时刻,通过对预测域的最后一步,求解垂直运动通道的终端惩罚加权矩阵Pz,确保在k>N时系统稳定;
Pz按照如下方式求解:
其中tr(Pz)为矩阵Pz的迹;
LMIi(Y,W)为约束条件,Y为第一指代值,Y=Pz -1;W为第二指代值,W=KPz -1,K为线性状态反馈控制系数;Qz为垂直运动通道的状态权重矩阵;Rz为垂直运动通道的输入权重矩阵;I为单位矩阵。
在求得终端惩罚矩阵Pz后,对垂直运动通道采用与水平运动通道相同的模型预测控制方法进行轨迹跟踪。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011183904.4A CN112241125B (zh) | 2020-10-29 | 2020-10-29 | 一种基于微分平坦特性的无人机轨迹跟踪方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011183904.4A CN112241125B (zh) | 2020-10-29 | 2020-10-29 | 一种基于微分平坦特性的无人机轨迹跟踪方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112241125A true CN112241125A (zh) | 2021-01-19 |
CN112241125B CN112241125B (zh) | 2021-08-13 |
Family
ID=74170064
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011183904.4A Active CN112241125B (zh) | 2020-10-29 | 2020-10-29 | 一种基于微分平坦特性的无人机轨迹跟踪方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112241125B (zh) |
Cited By (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112947572A (zh) * | 2021-03-12 | 2021-06-11 | 华南理工大学 | 一种基于地形跟随的四旋翼飞行器自适应运动规划方法 |
CN113190043A (zh) * | 2021-05-06 | 2021-07-30 | 国网山西省电力公司吕梁供电公司 | 基于ros平台的六旋翼无人机mpc控制方法 |
CN113359472A (zh) * | 2021-07-02 | 2021-09-07 | 北京理工大学 | 一种四旋翼无人机自适应鲁棒轨迹跟踪控制方法 |
CN113985927A (zh) * | 2021-10-28 | 2022-01-28 | 西北工业大学太仓长三角研究院 | 一种四旋翼无人机栖停机动轨迹优化方法 |
CN114253136A (zh) * | 2021-12-13 | 2022-03-29 | 哈尔滨工业大学 | 基于离散高阶全驱系统方法的组合卫星模拟器控制方法 |
CN114995521A (zh) * | 2022-08-08 | 2022-09-02 | 中国科学院自动化研究所 | 多无人机分布式编队控制方法、装置及电子设备 |
CN115366109A (zh) * | 2022-09-23 | 2022-11-22 | 北京航空航天大学杭州创新研究院 | 一种旋翼飞行机械臂复合分层抗干扰方法 |
WO2022241991A1 (zh) * | 2021-05-17 | 2022-11-24 | 北京理工大学 | 高超声速飞行器轨迹跟踪方法 |
CN115421506A (zh) * | 2022-03-28 | 2022-12-02 | 北京理工大学 | 一种基于模型预测控制的无人机周期轨迹跟踪及避障方法 |
CN116301026A (zh) * | 2023-01-13 | 2023-06-23 | 中国建筑一局(集团)有限公司 | 一种复杂环境下四旋翼无人机大机动敏捷飞行方法 |
CN117193346A (zh) * | 2023-10-26 | 2023-12-08 | 浙江大学 | 一种可进行自主导航的迷你共轴双旋翼无人机 |
CN118276444A (zh) * | 2024-03-29 | 2024-07-02 | 国网宁夏电力有限公司石嘴山供电公司 | 基于非线性模型预测的四旋翼无人机路径跟踪控制方法 |
CN118502250A (zh) * | 2024-07-16 | 2024-08-16 | 广东工业大学 | 针对无人机的数据处理系统、方法、装置及存储介质 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20050170759A1 (en) * | 2002-12-28 | 2005-08-04 | Boardman James A. | Differential planarization |
CN105467996A (zh) * | 2015-12-21 | 2016-04-06 | 北京理工大学 | 基于微分平坦和自抗扰的四轮转向汽车轨迹跟踪控制方法 |
CN107272692A (zh) * | 2017-07-18 | 2017-10-20 | 北京理工大学 | 基于微分平坦和自抗扰的无人车路径规划与跟踪控制方法 |
CN108445898A (zh) * | 2018-05-14 | 2018-08-24 | 南开大学 | 基于微分平坦特性的四旋翼无人飞行器系统运动规划方法 |
-
2020
- 2020-10-29 CN CN202011183904.4A patent/CN112241125B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20050170759A1 (en) * | 2002-12-28 | 2005-08-04 | Boardman James A. | Differential planarization |
CN105467996A (zh) * | 2015-12-21 | 2016-04-06 | 北京理工大学 | 基于微分平坦和自抗扰的四轮转向汽车轨迹跟踪控制方法 |
CN107272692A (zh) * | 2017-07-18 | 2017-10-20 | 北京理工大学 | 基于微分平坦和自抗扰的无人车路径规划与跟踪控制方法 |
CN108445898A (zh) * | 2018-05-14 | 2018-08-24 | 南开大学 | 基于微分平坦特性的四旋翼无人飞行器系统运动规划方法 |
Non-Patent Citations (2)
Title |
---|
卢鸿波: "不确定情况下欠驱动四旋翼无人机的控制系统研究", 《中国优秀硕士学位论文全文数据库(电子期刊)》 * |
黄敏: "基于微分平坦理论的四旋翼无人机轨迹规划的研究", 《中国优秀硕士学位论文全文数据库(电子期刊)》 * |
Cited By (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112947572A (zh) * | 2021-03-12 | 2021-06-11 | 华南理工大学 | 一种基于地形跟随的四旋翼飞行器自适应运动规划方法 |
CN113190043A (zh) * | 2021-05-06 | 2021-07-30 | 国网山西省电力公司吕梁供电公司 | 基于ros平台的六旋翼无人机mpc控制方法 |
WO2022241991A1 (zh) * | 2021-05-17 | 2022-11-24 | 北京理工大学 | 高超声速飞行器轨迹跟踪方法 |
CN113359472A (zh) * | 2021-07-02 | 2021-09-07 | 北京理工大学 | 一种四旋翼无人机自适应鲁棒轨迹跟踪控制方法 |
CN113359472B (zh) * | 2021-07-02 | 2024-04-19 | 北京理工大学 | 一种四旋翼无人机自适应鲁棒轨迹跟踪控制方法 |
CN113985927A (zh) * | 2021-10-28 | 2022-01-28 | 西北工业大学太仓长三角研究院 | 一种四旋翼无人机栖停机动轨迹优化方法 |
CN113985927B (zh) * | 2021-10-28 | 2023-11-21 | 西北工业大学太仓长三角研究院 | 一种四旋翼无人机栖停机动轨迹优化方法 |
CN114253136A (zh) * | 2021-12-13 | 2022-03-29 | 哈尔滨工业大学 | 基于离散高阶全驱系统方法的组合卫星模拟器控制方法 |
CN115421506A (zh) * | 2022-03-28 | 2022-12-02 | 北京理工大学 | 一种基于模型预测控制的无人机周期轨迹跟踪及避障方法 |
CN115421506B (zh) * | 2022-03-28 | 2023-08-11 | 北京理工大学 | 一种基于模型预测控制的无人机周期轨迹跟踪及避障方法 |
CN114995521A (zh) * | 2022-08-08 | 2022-09-02 | 中国科学院自动化研究所 | 多无人机分布式编队控制方法、装置及电子设备 |
CN115366109A (zh) * | 2022-09-23 | 2022-11-22 | 北京航空航天大学杭州创新研究院 | 一种旋翼飞行机械臂复合分层抗干扰方法 |
CN116301026A (zh) * | 2023-01-13 | 2023-06-23 | 中国建筑一局(集团)有限公司 | 一种复杂环境下四旋翼无人机大机动敏捷飞行方法 |
CN117193346A (zh) * | 2023-10-26 | 2023-12-08 | 浙江大学 | 一种可进行自主导航的迷你共轴双旋翼无人机 |
CN117193346B (zh) * | 2023-10-26 | 2024-04-30 | 浙江大学 | 一种可进行自主导航的迷你共轴双旋翼无人机 |
CN118276444A (zh) * | 2024-03-29 | 2024-07-02 | 国网宁夏电力有限公司石嘴山供电公司 | 基于非线性模型预测的四旋翼无人机路径跟踪控制方法 |
CN118502250A (zh) * | 2024-07-16 | 2024-08-16 | 广东工业大学 | 针对无人机的数据处理系统、方法、装置及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN112241125B (zh) | 2021-08-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112241125B (zh) | 一种基于微分平坦特性的无人机轨迹跟踪方法 | |
CN102073755B (zh) | 近空间高超声速飞行器运动控制仿真方法 | |
Raffo et al. | Backstepping/nonlinear H∞ control for path tracking of a quadrotor unmanned aerial vehicle | |
Das et al. | Backstepping approach for controlling a quadrotor using lagrange form dynamics | |
Mokhtari et al. | Extended state observer based control for coaxial-rotor UAV | |
CN108958289A (zh) | 基于相对速度障碍的集群无人机避碰方法 | |
CN106155076B (zh) | 一种多旋翼无人飞行器的稳定飞行控制方法 | |
Lu et al. | Real-time simulation system for UAV based on Matlab/Simulink | |
Lugo-Cárdenas et al. | Lyapunov based 3D path following kinematic controller for a fixed wing UAV | |
Oliveira et al. | Three dimensional moving path following for fixed-wing unmanned aerial vehicles | |
CN114911265A (zh) | 一种四旋翼无人机编队协同机动控制方法 | |
Jeong et al. | Control System Design for a Ducted‐Fan Unmanned Aerial Vehicle Using Linear Quadratic Tracker | |
CN107894775B (zh) | 一种欠驱动无人水下航行器轨迹生成与控制方法 | |
Kumar et al. | Differential flatness based hybrid PID/LQR flight controller for complex trajectory tracking in quadcopter UAVs | |
Zhu et al. | Trajectory linearization control for a miniature unmanned helicopter | |
Joukhadar et al. | Advanced UAVs nonlinear control systems and applications | |
Lee et al. | A distributed two-layer framework for teleoperated platooning of fixed-wing UAVs via decomposition and backstepping | |
Gümüşboğa et al. | Aircraft trim analysis by particle swarm optimization | |
Gritsenko et al. | Integral adaptive autopilot for an unmanned aerial vehicle | |
CN107247464B (zh) | 一种四旋翼无人飞行器的状态受限控制方法和系统 | |
Kumar et al. | Exponential reaching law based robust trajectory tracking for unmanned aerial vehicles | |
Kemper et al. | Impact of center of gravity in quadrotor helicopter controller design | |
CN112161626B (zh) | 一种基于航路跟踪映射网络的高可飞性航路规划方法 | |
Azimov et al. | Real-time guidance, navigation and control framework for fixed-wing aircraft maneuvers in a vertical plane | |
CN114167720A (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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |