一种四旋翼无人飞行器的状态受限控制方法和系统技术方案

本发明专利技术公开了一种四旋翼无人飞行器的状态受限控制方法，所述方法包括：步骤1)获取t时刻飞行器的位置信息、速度信息、姿态信息和角速度信息；步骤2)根据理想轨迹计算t时刻的位置跟踪误差和速度跟踪误差，并由此设计t+1时刻位置中间控制律F；步骤3)根据t+1时刻的位置中间控制率F，设计t+1时刻的控制升力T；步骤4)根据t+1时刻的控制升力T，计算飞行器t+1时刻的理想姿态和理想角速度；步骤5)根据理想姿态和理想角速度计算姿态误差和角速度误差；步骤6)设计t+1时刻的姿态中间控制律β，并由此设计t+1时刻的控制力矩Γ，使飞行器在t+1时刻达到理想姿态和理想角速度。本发明专利技术的方法利用单位四元数对飞行器姿态进行描述，提高了计算效率；并提高了控制精度。

A state constrained control method and system for four rotor unmanned aerial vehicle

The invention discloses a control method of state constrained four rotor UAV, the method includes: Step 1) to obtain t time vehicle location information, speed information, attitude information and velocity information; step 2) calculation of T position tracking error and tracking error according to the desired trajectory, and thus the design of t+1 the middle position control law F; step 3) according to the position of intermediate time t+1 control F, t+1 design time control lift T; step 4) according to t+1 moment control lift T, calculate the ideal aircraft attitude t+1 moment and ideal angular velocity; step 5) calculation of attitude error and angular velocity error according to the ideal the ideal attitude and angular velocity; step 6) attitude control law design middle time t+1 beta, and thus the design of control moment gamma t+1 moment of the vehicle, to achieve the ideal posture and the ideal angle at time t+1 Speed. The method uses the unit four element number to describe the attitude of the aircraft, improves the calculation efficiency, and improves the control precision.

【技术实现步骤摘要】
一种四旋翼无人飞行器的状态受限控制方法和系统
本专利技术涉及无人飞行器领域，具体涉及一种四旋翼无人飞行器的状态受限控制方法和系统。
技术介绍
四旋翼无人飞行器是一种由四个电机驱动旋翼飞行的小型无人飞行器，具有机动性强、结构紧凑、可以垂直起降以及空中悬停等优点，近年来在诸多领域得到了广泛的应用。同时，由于其系统内部所具有的欠驱动特性：控制输入具有四个而被控量具有六个，使得对四旋翼无人飞行器控制系统的设计分析具有一定的难度，因此在理论研究方面，四旋翼无人飞行器的控制器设计与分析也是一个研究热点。现有的四旋翼无人飞行器的模型大多采用欧拉角来对姿态进行描述，会发生欧拉奇异现象，使控制律失效；同时，欧拉角进行姿态描述会引入三角函数运算，使得系统的分析更为复杂；其次，现有的控制方法仅能够保证误差的收敛性能，而对动态过程的误差变化情况没有进行考虑，虽然系统稳定，但是误差在跟踪过程时可能已经超出了容许范围，不能满足控制要求，使得控制方案失效；此外，在常规的控制技术中，对系统的模型进行了简化，不仅引入了建模误差，而且仅仅保证了位置和姿态的各自的稳定性，并没有对位置与姿态之间的耦合进行分析，闭环系统的稳定性没有得到证明。
技术实现思路
本专利技术的目的在于克服目前四旋翼无人飞行器的控制方法存在的上述缺陷，考虑到单位四元数具有计算效率高，便于系统的设计与分析等优点，本专利技术的方法采用单位四元数对飞行器姿态进行描述，避免了欧拉奇异现象，在设计控制律时引入了barrier约束函数，使得系统的误差被限制在指定的范围内，保证了轨迹跟踪的精度。为实现上述目的，本专利技术提出了一种四旋翼无人飞行器的状态受限控制方法，所述方法包括：步骤1)获取t时刻飞行器的位置信息、速度信息、姿态信息和角速度信息；步骤2)根据理想轨迹计算t时刻的位置跟踪误差和速度跟踪误差，并由此设计t+1时刻位置中间控制律F；步骤3)根据t+1时刻的位置中间控制率F，设计t+1时刻的控制升力T；步骤4)根据t+1时刻的控制升力T，计算飞行器t+1时刻的理想姿态和理想角速度；步骤5)根据理想姿态和理想角速度计算姿态误差和角速度误差；步骤6)设计t+1时刻的姿态中间控制律β，并由此设计t+1时刻的控制力矩Γ，使飞行器在t+1时刻达到理想姿态和理想角速度。上述技术方案中，所述步骤2)具体为：设t时刻的飞行器的位置信息为p，速度信息为v，理想轨迹信息分别为pd与vd，定义位置跟踪误差为速度跟踪误差为则t+1时刻位置中间控制律F为：其中kb1,kb2＞0，为位置跟踪误差与速度跟踪误差所容许的最大设定值，kz＞0为控制增益。上述技术方案中，所述步骤3)具体为：将F投影到惯性坐标系中表示为：F＝(Fx,Fy,Fz)T，则控制升力T为：其中，m为飞行器的质量。上述技术方案中，所述步骤4)具体为：定义单位四元数为四旋翼飞行器的理想姿态，ηd为Qd的标量部分，ωd为飞行器的理想角速度；则ηd为：理想角速度为ωd为：其中，I3为三阶的单位对角矩阵，S(qd)为3×3斜对称矩阵：上述技术方案中，所述步骤5)具体为：设t时刻的飞行器的姿态信息为单位四元数Q＝(qT,η)T＝(q1,q2,q3,η)T，q＝(q1,q2,q3)T为Q的矢量部分，η为Q的标量部分；角速度信息为ω＝(ωx,ωy,ωz)T；以飞行器的姿态Q建立的坐标系为飞行器的本体坐标系，以飞行器的理想姿态Qd建立的坐标系为飞行器的理想坐标系，则飞行器的本体坐标系与理想坐标系之间的姿态误差以及角速度误差为：其中，为理想坐标系到机体坐标系的旋转矩阵，其中：R(Q)＝(η2-||q||2)I3+2qqT-2ηS(q)R(Qd)＝(ηd2-||qd||2)I3+2qdqdT-2ηdS(qd)Q＝(qT,η)T＝(q1,q2,q3,η)T，q＝(q1,q2,q3)T；ω＝(ωx,ωy,ωz)T。上述技术方案中，所述步骤6)具体为：定义矩阵为：If为惯性矩阵，设计的姿态中间控制律为：其中，kβ＞0为控制增益，列向量设计控制力矩Γ为：其中，中间变量Ω：kΩ＞0为控制增益，M1为对角矩阵：设t＝0时，的前三项分别为：则kb31、kb32、kb33都为大于零的常数，且满足一种四旋翼无人飞行器的状态受限控制系统，包括存储器、处理器和存储在存储器上的并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现上述方法的步骤。本专利技术的优势在于：1、本专利技术的方法利用单位四元数对飞行器姿态进行描述，提高了计算效率，避免了奇异现象；2、根据在某些狭窄环境下(如山洞、狭小的室内空间)对四旋翼无人机轨迹跟踪任务的精度要求，此时需要限制无人机的速度以及位置，保证轨迹跟踪的精度，本专利技术的方法通过引入barrier约束函数，设计控制器(分为位置控制器以及姿态控制器)，保证位置追踪误差以及速度追踪误差的精度要求，从而间接避免了碰撞的发生。附图说明图1为四旋翼飞行器结构图；图2a为仿真实例的x轴位置误差跟踪图；图2b为仿真实例的y轴位置误差跟踪图；图2c为仿真实例的z轴位置误差跟踪图；图3a为仿真实例的x轴速度误差跟踪图；图3b为仿真实例的y轴速度误差跟踪图；图3c为仿真实例的z轴速度误差跟踪图；图4为仿真实例的空间三维轨迹跟踪图；图5a为仿真实例的四元数qd1跟踪图；图5b为仿真实例的四元数qd2跟踪图；图5c为仿真实例的四元数qd3跟踪图；图5d为仿真实例的四元数ηd跟踪图。具体实施方式下面结合附图和具体实施例对本专利技术进行详细的说明。一种四旋翼无人飞行器的状态受限控制方法，所述方法包括：步骤1)建立四旋翼无人机飞行器的动力学方程；如图1所示，定义方向为北、东、地的坐标系E＝{e1,e2,e3}为惯性参考系；定义原点在四旋翼几何中心方向为前、右、下的坐标系B＝{b1,b2,b3}为本体坐标系，均满足右手定则。为了避免欧拉奇异现象且提高运算效率，使用单位四元数描述飞行器的姿态。定义单位四元数为其中称为单位四元数矢量部分，称为单位四元数标量部分，单位四元数的各个分量满足qTq+η2＝1。为从惯性坐标系到本体坐标系的旋转矩阵，定义为：R(Q)＝(η2-||q||2)I3+2qqT-2ηS(q)(1)其中I3为三阶的单位对角矩阵，||·||为向量欧式范数。S(q)为3×3斜对称矩阵：四旋翼飞行器的结构如图1所示，其运动学以及动力学微分方程表示为：称式(2)为位置子系统，式(3)为姿态子系统。其中，m为飞行器质量，g为重力加速度，为惯性矩阵，为机体角速度。为待设计的四旋翼飞行器输入升力，为待设计的输入转矩。观察式(2)可知，控制输入为T，而系统状态变量为p，控制量的维数小于系统状态空间的维数，因此位置子系统是一个欠驱动系统。观察式(3)可知，控制输入为Γ，飞行器姿态变量为单位四元数Q＝(qT,η)T＝(q1,q2,q3,η)T，q＝(q1,q2,q3)T为Q的矢量部分，η为Q的标量部分；角速度信息为ω＝(ωx,ωy,ωz)T；，该系统是全驱动的。为了使系统能够跟踪一个三维的理想轨迹一个合理的控制目标为保证位置p＝(px,py,pz)T以及某一个姿态角对指令的跟踪，其余的两个姿态角保持镇定或者随动。定义为四旋翼飞行器的理想姿态，ωd为机体的理想角速度。本体坐标系与理想坐标系之间本文档来自技高网...

【技术保护点】
一种四旋翼无人飞行器的状态受限控制方法，所述方法包括：步骤1)获取t时刻飞行器的位置信息、速度信息、姿态信息和角速度信息；步骤2)根据理想轨迹计算t时刻的位置跟踪误差和速度跟踪误差，并由此设计t+1时刻位置中间控制律F；步骤3)根据t+1时刻的位置中间控制率F，设计t+1时刻的控制升力T；步骤4)根据t+1时刻的控制升力T，计算飞行器t+1时刻的理想姿态和理想角速度；步骤5)根据理想姿态和理想角速度计算姿态误差和角速度误差；步骤6)设计t+1时刻的姿态中间控制律β，并由此设计t+1时刻的控制力矩Γ，使飞行器在t+1时刻达到理想姿态和理想角速度。

【技术特征摘要】
1.一种四旋翼无人飞行器的状态受限控制方法，所述方法包括：步骤1)获取t时刻飞行器的位置信息、速度信息、姿态信息和角速度信息；步骤2)根据理想轨迹计算t时刻的位置跟踪误差和速度跟踪误差，并由此设计t+1时刻位置中间控制律F；步骤3)根据t+1时刻的位置中间控制率F，设计t+1时刻的控制升力T；步骤4)根据t+1时刻的控制升力T，计算飞行器t+1时刻的理想姿态和理想角速度；步骤5)根据理想姿态和理想角速度计算姿态误差和角速度误差；步骤6)设计t+1时刻的姿态中间控制律β，并由此设计t+1时刻的控制力矩Γ，使飞行器在t+1时刻达到理想姿态和理想角速度。2.根据权利要求1所述的四旋翼无人飞行器的状态受限控制方法，其特征在于，所述步骤2)具体为：设t时刻的飞行器的位置信息为p，速度信息为v，理想轨迹信息分别为pd与vd，定义位置跟踪误差为速度跟踪误差为则t+1时刻位置中间控制律F为：其中kb1,kb2＞0，为位置跟踪误差与速度跟踪误差所容许的最大设定值，kz＞0为控制增益。3.根据权利要求2所述的四旋翼无人飞行器的状态受限控制方法，其特征在于，所述步骤3)具体为：将F投影到惯性坐标系中表示为：F＝(Fx,Fy,Fz)T，则控制升力T为：其中，m为飞行器的质量。4.根据权利要求3所述的四旋翼无人飞行器的状态受限控制方法，其特征在于，所述步骤4)具体为：定义单位四元数为四旋翼飞行器的理想姿态，ηd为Qd的标量部分，ωd为飞行器的理想角速度；则ηd为：根据单位四元数运动学方程，理想角速度为ωd为：1其中，I3为三阶的单位对角矩阵，S(qd)为3×3斜对称矩阵：5.根据权利要求4所述的四旋翼无人飞行器的状态受限控制方法，其特征在于，所述步骤5)具体为：设t时刻的飞行器的姿态信息为单位四元数Q＝(qT,η)T＝(q1,q2,q3,η)T，q＝(q1...

【专利技术属性】
技术研发人员：王锐，
申请(专利权)人：中国科学院声学研究所，
类型：发明
国别省市：北京,11