CN117055602A - 一种四旋翼无人机的全姿态控制方法 - Google Patents

Abstract

本发明适用于无人机领域，提供了一种四旋翼无人机的全姿态控制方法。与现有技术相比，本发明的有益效果在于：稳定性更好，不存在使用欧拉角时会出现的极点或数值漂移的问题，通过对数值进行的稳定，保证了控制时的稳定性；健壮性更强，可以适应所有姿态的刚体运行控制情形，无姿态控制的限制，对大多数的大角度机动和极点附件的运动，不会出现控制失效的问题；计算更快速且易于优化，相对于现有的欧拉角方案，三角函数等需要迭代的运算的使用频次低，也并非关键步骤，易于通过近似进行优化。

Description

一种四旋翼无人机的全姿态控制方法

技术领域

本发明适用于无人机领域，尤其涉及一种四旋翼无人机的全姿态控制方法。

背景技术

四旋翼无人机在起飞后通常需要进行基础的姿态控制，以方便后续具体应用的开展，对于姿态变化较大的四旋翼无人机，通常在控制时会因为姿态表示而造成控制上的困难，其控制器设计是较为棘手的，因此对于特殊场景，设计特殊的全姿态控制方案具有极大的开创性和实用价值。

全姿态控制目前存在如下的难点：欧拉角对姿态的表示并不唯一，因此会限制飞行器的姿态范围。虽然使用四元数进行姿态表示可以一定程度上解决上述问题，但由于四元数的表达不直观，在最终的控制阶段依然需要使用欧拉角进行控制，由此会造成无人机在姿态变化较大时出现控制数值不稳定甚至解算错误的问题。因此现有的技术方案难以绕开欧拉角，而在大姿态角度条件下，欧拉角的控制方案轻则出现求解数值不稳定，重则无法进行准确的控制。

发明内容

本发明提供一种四旋翼无人机的全姿态控制方法，旨在解决现有技术中解决过程高度依赖欧拉角，数值稳定性不佳以及大角度下解算错误的问题。

所述全姿态控制方法包括以下步骤：

S1、获取所述四旋翼无人机的目标姿态，对所述目标姿态进行预处理，得到预处理目标姿态；

S2、通过对所述预处理目标姿态进行计算，得到期望姿态；通过对所述期望姿态进行计算，得到所述四旋翼无人机的旋转方向和旋转角度；

S3、获取所述四旋翼无人机的角速度原始值；

S4、通过对所述角速度原始值进行计算，得到角速度增益系数；

S5、通过对所述角速度增益系数进行调整，得到所述四旋翼无人机的期望角速度；

S6、通过PID控制器跟踪所述期望角速度，得到目标力矩；

S7、根据所述目标力矩对所述四旋翼无人机的姿态进行控制。

优选的，步骤S1中，所述预处理目标姿态的计算公式如下：

其中，q_real表示所述预处理目标姿态，表示四元数的乘法，q_raw表示所述四旋翼无人机无姿态变化时的四元数姿态，q^*表示四元数q的共轭值，q_t表示所述四旋翼无人机姿态控制时的四元数姿态。

优选的，步骤S2中，所述旋转方向的计算公式如下：

q_err＝[qe₀ qe₁ qe₂ qe₃]；

V＝[qe₁ qe₂ qe₃]^T＝[V_x V_y V_z]^T；

其中，V表示所述旋转方向，V_x、V_y和V_z分别表示旋转向量在x、y和z轴的分量大小，q_err表示所述期望姿态，qe₀、qe₁、qe₂、qe₃均表示所述期望姿态q_err的内部数值。

优选的，步骤S2中，所述旋转角度的计算公式如下：

其中，α表示预设常数，θ表示所述旋转角度。

优选的，步骤S3中，所述角速度原始值的计算公式如下：

其中，S表示常数，用于消除极点，ω_raw表示所述角速度原始值，q_x、q_y、q_z、q_w均表示所述预处理目标姿态q_real的内部数值。

优选的，步骤S4中，得到所述角速度增益系数的计算公式如下：

其中，k表示所述角速度增益系数，ω_x、ω_y、ω_z分别表示。

优选的，步骤S5中，得到所述期望角速度的计算公式如下：

ω＝kω_raw；

其中，ω表示所述期望角速度。

优选的，步骤S6中，得到所述目标力矩的计算公式如下：

M＝G(ω-ω_now)；

其中，M表示所述目标力矩，G表示角速度跟踪方法，ω_now表示测量或估计得到的角速度。

与现有技术相比，本发明的有益效果在于：稳定性更好，不存在使用欧拉角时会出现的极点或数值漂移的问题，通过对数值进行的稳定，保证了控制时的稳定性；健壮性更强，可以适应所有姿态的刚体运行控制情形，无姿态控制的限制，对大多数的大角度机动和极点附件的运动，不会出现控制失效的问题；计算更快速且易于优化，相对于现有的欧拉角方案，三角函数等需要迭代的运算的使用频次低，也并非关键步骤，易于通过近似进行优化。

附图说明

下面结合附图详细说明本发明。通过结合以下附图所作的详细描述，本发明的上述或其他方面的内容将变得更清楚和更容易理解。附图中：

图1是本发明实施例提供的四旋翼无人机的全姿态控制方法的流程框图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

请参考图1，图1是本发明实施例提供的四旋翼无人机的全姿态控制方法的流程框图，本发明提供一种四旋翼无人机的全姿态控制方法，所述全姿态控制方法包括以下步骤：

S1、获取所述四旋翼无人机的目标姿态，对所述目标姿态进行预处理，得到预处理目标姿态；

在本发明实施例中，对于姿态控制器，可以从位置控制器获取到位置控制所需的加速度信息：

acc＝[a_x a_y a_x]；

同时从外部给定得到机体需要达到的偏航角的角度：θ_yaw，且重力加速度为：g；最佳情况下，四旋翼无人机的拉力为：

F＝[a_x a_y g+a_z]m；

式中：m为无人机质量；

为上述拉力设定限幅：

g+a_z≥1；

如果上述条件无法达成，可设定为：g+a_z＝1，以防止计算出现不稳定。达成上述拉力时，四旋翼无人机的姿态需要满足一定条件，则可以使用下面的等式进行计算：

其中，符号为四元数乘法，上述计算得到的姿态为目标姿态，和现有方法相比，可以对带有偏航角的情况进行计算。

获取四旋翼无人机处于静态、无姿态变换下的四元数姿态q_raw，并在需要进行姿态控制时采集到四元数姿态q_t，由此可进行计算得到近似实际四元数姿态，即所述预处理目标姿态q_real：

式中：q^*表示四元数q的共轭值，定义为四元数的乘法。

定义上述所述预处理目标姿态q_real的内部数值为q_x、q_y、q_z、q_w则：

q_real＝[q_w q_x q_y q_z]；

通过考察q_w，使得姿态表示的旋转方向相同，若q_w<0则表示该四元数形式的姿态表示的旋转方向和q_w>0时不同。

基于上述考察，当q_w<0，对q_real的数值取相反数，式中←符号表示数值更新迭代：

q_real←[-q_w -q_x -q_y -q_z],if:q_w<0；

将四元数姿态简化为单位四元数：

式中：|q|为四元数q的模值。

S2、通过对所述预处理目标姿态进行计算，得到期望姿态；通过对所述期望姿态进行计算，得到所述四旋翼无人机的旋转方向和旋转角度；

在本发明实施例中，对于所述四旋翼无人机当前的所述预处理目标姿态q_real，需要变换到目标的姿态q_target，可计算得到期望姿态：

对于期望姿态q_err，定义其内部数值如下：

q_err＝[qe₀ qe₁ qe₂ qe₃]；

计算得到上述期望姿态q_err对应的旋转向量，即控制预期的旋转向量：

V＝[qe₁ qe₂ qe₃]^T＝[V_x V_y V_z]^T；

式中：V_x、V_y和V_z表示旋转向量在x、y和z轴的分量大小，qe₀、qe₁、qe₂、qe₃均表示所述期望姿态q_err的内部数值。

定义常量α→1且α<1，该数值表达的含义是：对于四旋翼无人机的控制周期T，四旋翼无人机的机体平均角速度为

得到控制预期的旋转角度为：

同时，上述计算也可简化为：

S3、获取所述四旋翼无人机的角速度原始值；

在本发明实施例中，根据上述步骤得到的数值，计算出以下关于控制预期角速度的方程组：

式中：ω_x、ω_y和ω_z表示在x、y和z轴上需要的控制预期角速度，为待求量。

定义角速度原始值为ω_raw，表示控制预期的各个旋转轴的角速度：

ω_raw＝[ω_x ω_y ω_z]^T；

提取并分离上述方程组的未知和已知量以简化计算：

式中：A为辅助计算的矩阵。

求解方程组得到：

式中：A^*表示矩阵A的伴随矩阵。

在实际使用过程中，为保证稳定性，消去单位四元数表示下的常量，并作极点消除处理：

式中：S为一个大于1.5的常数，用于消除极点。

S4、通过对所述角速度原始值进行计算，得到角速度增益系数；

在本发明实施例中，计算角速度增益系数的计算公式如下：

式中：k称为角速度增益系数，是调整ω_raw数值大小的参数。可以证明的是该数值在调整ω_raw数值大小时不会改变其关键指标。

S5、通过对所述角速度增益系数进行调整，得到所述四旋翼无人机的期望角速度；

在本发明实施例中，调整得到控制所需要的期望角速度的计算公式如下：

ω＝kω_raw。

S6、通过PID控制器跟踪所述期望角速度，得到目标力矩；

在本发明实施例中，使用PID控制器跟踪上述期望角速度，可以获取到控制过程中需要的目标力矩M，目标力矩M计算公式如下：

M＝G(ω-ω_now)；

式中：G为角速度跟踪方法；ω_now为测量或估计得到的角速度，当无法获取精确的角速度时，可以设定：ω_now＝0；根据得到的目标力矩，即可进行所述四旋翼无人机的姿态控制。

S7、根据所述目标力矩对所述四旋翼无人机的姿态进行控制。

与现有技术相比，本发明的有益效果在于：稳定性更好，不存在使用欧拉角时会出现的极点或数值漂移的问题，通过对数值进行的稳定，保证了控制时的稳定性；健壮性更强，可以适应所有姿态的刚体运行控制情形，无姿态控制的限制，对大多数的大角度机动和极点附件的运动，不会出现控制失效的问题；计算更快速且易于优化，相对于现有的欧拉角方案，三角函数等需要迭代的运算的使用频次低，也并非关键步骤，易于通过近似进行优化。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。

上面结合附图对本发明的实施例进行了描述，所揭露的仅为本发明较佳实施例而已，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式用等同变化，均属于本发明的保护之内。

Claims (8)

1.一种四旋翼无人机的全姿态控制方法，其特征在于，所述全姿态控制方法包括以下步骤：

S1、获取所述四旋翼无人机的目标姿态，对所述目标姿态进行预处理，得到预处理目标姿态；

S2、通过对所述预处理目标姿态进行计算，得到期望姿态；通过对所述期望姿态进行计算，得到所述四旋翼无人机的旋转方向和旋转角度；

S3、获取所述四旋翼无人机的角速度原始值；

S4、通过对所述角速度原始值进行计算，得到角速度增益系数；

S5、通过对所述角速度增益系数进行调整，得到所述四旋翼无人机的期望角速度；

S6、通过PID控制器跟踪所述期望角速度，得到目标力矩；

S7、根据所述目标力矩对所述四旋翼无人机的姿态进行控制。

2.如权利要求1所述的四旋翼无人机的全姿态控制方法，其特征在于，步骤S1中，所述预处理目标姿态的计算公式如下：

其中，q_real表示所述预处理目标姿态，表示四元数的乘法，q_raw表示所述四旋翼无人机无姿态变化时的四元数姿态，q^*表示四元数q的共轭值，q_t表示所述四旋翼无人机姿态控制时的四元数姿态。

3.如权利要求2所述的四旋翼无人机的全姿态控制方法，其特征在于，步骤S2中，所述旋转方向的计算公式如下：

q_err＝[qe₀ qe₁ qe₂ qe₃]；

V＝[qe_l qe₂ qe₃]^T＝[V_x V_y V_z]^T；

其中，V表示所述旋转方向，V_x、V_y和V_z分别表示旋转向量在x、y和z轴的分量大小，q_err表示所述期望姿态，qe₀、qe₁、qe₂、qe₃均表示所述期望姿态q_err的内部数值。

4.如权利要求3所述的四旋翼无人机的全姿态控制方法，其特征在于，步骤S2中，所述旋转角度的计算公式如下：

其中，α表示预设常数，θ表示所述旋转角度。

5.如权利要求4所述的四旋翼无人机的全姿态控制方法，其特征在于，步骤S3中，所述角速度原始值的计算公式如下：

其中，S表示常数，用于消除极点，ω_raw表示所述角速度原始值，q_x、q_y、q_z、q_w均表示所述预处理目标姿态q_real的内部数值。

6.如权利要求5所述的四旋翼无人机的全姿态控制方法，其特征在于，步骤S4中，得到所述角速度增益系数的计算公式如下：

其中，k表示所述角速度增益系数，ω_x、ω_y、ω_z分别表示在x、y和z轴上所需的控制预期角速度。

7.如权利要求6所述的四旋翼无人机的全姿态控制方法，其特征在于，步骤S5中，得到所述期望角速度的计算公式如下：

ω＝kω_raw；

其中，ω表示所述期望角速度。

8.如权利要求7所述的四旋翼无人机的全姿态控制方法，其特征在于，步骤S6中，得到所述目标力矩的计算公式如下：

M＝G(ω-ω_now)；

其中，M表示所述目标力矩，G表示角速度跟踪方法，ω_now表示测量或估计得到的角速度。