一种四旋翼无人飞行器的状态受限控制方法和系统与流程

本发明涉及无人飞行器领域，具体涉及一种四旋翼无人飞行器的状态受限控制方法和系统。

背景技术：

四旋翼无人飞行器是一种由四个电机驱动旋翼飞行的小型无人飞行器，具有机动性强、结构紧凑、可以垂直起降以及空中悬停等优点，近年来在诸多领域得到了广泛的应用。同时，由于其系统内部所具有的欠驱动特性：控制输入具有四个而被控量具有六个，使得对四旋翼无人飞行器控制系统的设计分析具有一定的难度，因此在理论研究方面，四旋翼无人飞行器的控制器设计与分析也是一个研究热点。

现有的四旋翼无人飞行器的模型大多采用欧拉角来对姿态进行描述，会发生欧拉奇异现象，使控制律失效；同时，欧拉角进行姿态描述会引入三角函数运算，使得系统的分析更为复杂；其次，现有的控制方法仅能够保证误差的收敛性能，而对动态过程的误差变化情况没有进行考虑，虽然系统稳定，但是误差在跟踪过程时可能已经超出了容许范围，不能满足控制要求，使得控制方案失效；此外，在常规的控制技术中，对系统的模型进行了简化，不仅引入了建模误差，而且仅仅保证了位置和姿态的各自的稳定性，并没有对位置与姿态之间的耦合进行分析，闭环系统的稳定性没有得到证明。

技术实现要素：

本发明的目的在于克服目前四旋翼无人飞行器的控制方法存在的上述缺陷，考虑到单位四元数具有计算效率高，便于系统的设计与分析等优点，本发明的方法采用单位四元数对飞行器姿态进行描述，避免了欧拉奇异现象，在设计控制律时引入了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，为位置跟踪误差与速度跟踪误差所容许的最大设定值，k^z＞0为控制增益。

上述技术方案中，所述步骤3)具体为：

将f投影到惯性坐标系中表示为：f＝(fx,fy,fz)^t，则控制升力t为：

其中，m为飞行器的质量。

上述技术方案中，所述步骤4)具体为：

定义单位四元数为四旋翼飞行器的理想姿态，ηd为qd的标量部分，ωd为飞行器的理想角速度；

则ηd为：

理想角速度为ωd为：

其中，i3为三阶的单位对角矩阵，s(qd)为3×3斜对称矩阵：

上述技术方案中，所述步骤5)具体为：

设t时刻的飞行器的姿态信息为单位四元数q＝(q^t,η)^t＝(q1,q2,q3,η)^t，q＝(q1,q2,q3)^t为q的矢量部分，η为q的标量部分；角速度信息为ω＝(ωx,ωy,ωz)^t；

以飞行器的姿态q建立的坐标系为飞行器的本体坐标系，以飞行器的理想姿态qd建立的坐标系为飞行器的理想坐标系，则飞行器的本体坐标系与理想坐标系之间的姿态误差以及角速度误差为：

其中，为理想坐标系到机体坐标系的旋转矩阵，其中：

r(q)＝(η²-||q||²)i3+2qq^t-2ηs(q)

r(qd)＝(ηd²-||qd||²)i3+2qdqd^t-2ηds(qd)

q＝(q^t,η)^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}为本体坐标系，均满足右手定则。为了避免欧拉奇异现象且提高运算效率，使用单位四元数描述飞行器的姿态。定义单位四元数为其中称为单位四元数矢量部分，称为单位四元数标量部分，单位四元数的各个分量满足q^tq+η²＝1。为从惯性坐标系到本体坐标系的旋转矩阵，定义为：

r(q)＝(η²-||q||²)i3+2qq^t-2ηs(q)(1)

其中i3为三阶的单位对角矩阵，||·||为向量欧式范数。s(q)为3×3斜对称矩阵：

四旋翼飞行器的结构如图1所示，其运动学以及动力学微分方程表示为：

称式(2)为位置子系统，式(3)为姿态子系统。其中，m为飞行器质量，g为重力加速度，为惯性矩阵，为机体角速度。为待设计的四旋翼飞行器输入升力，为待设计的输入转矩。

观察式(2)可知，控制输入为t，而系统状态变量为p，控制量的维数小于系统状态空间的维数，因此位置子系统是一个欠驱动系统。观察式(3)可知，控制输入为γ，飞行器姿态变量为单位四元数q＝(q^t,η)^t＝(q1,q2,q3,η)^t，q＝(q1,q2,q3)^t为q的矢量部分，η为q的标量部分；角速度信息为ω＝(ωx,ωy,ωz)^t；，该系统是全驱动的。为了使系统能够跟踪一个三维的理想轨迹一个合理的控制目标为保证位置p＝(px,py,pz)^t以及某一个姿态角对指令的跟踪，其余的两个姿态角保持镇定或者随动。

定义为四旋翼飞行器的理想姿态，ωd为机体的理想角速度。本体坐标系与理想坐标系之间的姿态误差以及角速度误差为：

其中，为理想坐标系到机体坐标系的旋转矩阵，其导数关系满足其中：

r(q)＝(η²-||q||²)i3+2qq^t-2ηs(q)

r(qd)＝(ηd²-||qd||²)i3+2qdqd^t-2ηds(qd)

四元数q＝(q^t,η)^t＝(q1,q2,q3,η)^t，q＝(q1,q2,q3)^t；角速度信息为ω＝(ωx,ωy,ωz)^t。

步骤2)设计中间控制律；

由位置子系统的微分方程以及图2可知，t的方向总是与本体坐标系的竖轴b3共线，仅仅通过t无法同时跟踪三维的理想轨迹。因此，在特定时刻四旋翼飞行器要具有某些特定的姿态，从而利用理想姿态qd将升力t投影到惯性坐标系的各个轴上以产生分量。

将升力t结合该时刻所需要的理想姿态qd，可以合成一个三维的控制力为

定义位置跟踪误差为速度跟踪误差为设计位置中间控制律f为

其中kb1,kb2＞0，为位置误差与速度误差所容许的最大设定值，k^z＞0为控制增益。利用中间控制律(6)，可以使得系统误差最终满足

其中，下标“j”为向量在惯性坐标系下x，y，z方向的分量。

步骤3)控制升力t的求解

式(6)表示了f实际值的变化，在获取了位置信息p与速度信息v后，与理想轨迹信息pd与vd分别做差可以得到位置误差与从而可知f在各个时刻的取值。但是f仅仅是一个虚拟的中间控制力，是由t与qd通过式(5)的运算关系合成得到，因此若想将式(6)中的f作用于系统，必须要通过式(5)反解出实际的控制升力t的大小，并由电机驱动产生升力t实现对系统的控制。

为了解出t，要用到f在惯性坐标系各坐标轴上的分量，投影到惯性坐标系中表示为：f＝(fx,fy,fz)^t。将式(5)展开为

将位置中间控制律f代入式(2)可得：

其中：

表示当前四旋翼无人机的姿态q与理想姿态qd对位置子系统造成的影响，对旋转矩阵造成的误差为

对于已知的(fx,fy,fz)^t，要通过上式来解出目标姿态以及实际升力t，共五个未知量。又因为qd满足单位向量的约束，因此上述方程组实际具有四个未知量，没有唯一解。为了得到一组解，可以固定qd中的某个分量，不失一般性，令qd3＝0，则：

将上式前两行的平方相加，可得：

通过配方法解方程式(12)，可以得到一个解为：

由于单位四元数的模值为1，代入式(11)可得：

将式(13)代入式(14)可得：

因此，在完成由式(6)所描述f的设计后，可以解出实际的控制升力t为：

步骤4)理想姿态的求解

将式(16)代入式(13)，解出ηd为：

结合式(11)的前两个方程，可以解出：

根据单位四元数运动学方程，可以得到理想角速度为ωd为：

步骤5)控制力矩γ的设计；

下面设计控制力矩γ来跟踪步骤4)所得到的理想姿态。

根据式(3)中的四元数运动学方程及式(4)中姿态误差的定义，可以建立模型的姿态误差系统如下：

其中，矩阵定义为：

if为惯性矩阵；

引入姿态中间控制律β，令

求导可得

则

设计姿态中间控制律为：

其中，k^β＞0为控制增益，列向量

最终设计控制力矩γ为：

其中，k^ω＞0为控制增益，对角矩阵

设t＝0时，的前三项分别为：则kb31、kb32、kb33都为大于零的常数，且满足

稳定性分析：

由式(2)与式(3)所描述的四旋翼无人飞行器模型，控制输入按式(6)、式(16)以及式(25)、(26)给定，当初始误差以及满足

可以证明闭环系统所有信号均有界，渐进收敛到零点，且

证明如下：

构造lyapunov函数

对式(30)求导得

其中，kb31，kb32，kb33为大于零的常数。

将控制律式(6)、式(16)以及式(25)、(26)代入式(31)，得

又因为m1正定对角矩阵，因此又因为k^z、α、k^β、k^ω均大于零，当时，有ω≡0，因此将结果代入方程(26)代入姿态子系统误差方程(22)可知将结果代入位置子系统误差方程可知

因此，闭环系统的平衡点为r(q)＝i3。根据lasalle不变集原理可知，当t→∞时，

引理1：针对误差动态系统式(34)：

其中存在连续可微并正定的函数v1和v2，kbi＞0,i＝1,2。例如设位置为x1，速度为x2，定义位置误差e1＝x1-yd，速度误差满足ei→-kbi或ei→kbi时，有vi(zi)→∞。假设|e1(0)|＜kbi，取如果满足式(35)

则|ei(t)|＜kbi,

根据引理1，式(29)可以得到证明。

仿真验证：

取质量为m＝0.5kg；重力加速度为g＝9.8m/s²；惯性矩阵为：

if＝diag(0.039,0.039,0.012)kg·m²。控制增益为：k^z＝5，kb1＝0.6，kb2＝0.3，kb31＝0.6，kb32＝0.6，kb33＝0.6，k^β＝20，k^ω＝10。系统变量初始状态为：

p(0)＝(0,0,0)^tm，v(0)＝(0,0,0)^tm/s，

参考轨迹为：pd＝(0.5cos(t),0.5sin(t),t/10)^tm。仿真结果如下图2a、图2b、图2c、图3a、图3b、图3c、图4、图5a、图5b、图5c和图5d。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。