本发明适用于无人机领域,尤其涉及一种四旋翼无人机的全姿态控制方法。
背景技术:
1、四旋翼无人机在起飞后通常需要进行基础的姿态控制,以方便后续具体应用的开展,对于姿态变化较大的四旋翼无人机,通常在控制时会因为姿态表示而造成控制上的困难,其控制器设计是较为棘手的,因此对于特殊场景,设计特殊的全姿态控制方案具有极大的开创性和实用价值。
2、全姿态控制目前存在如下的难点:欧拉角对姿态的表示并不唯一,因此会限制飞行器的姿态范围。虽然使用四元数进行姿态表示可以一定程度上解决上述问题,但由于四元数的表达不直观,在最终的控制阶段依然需要使用欧拉角进行控制,由此会造成无人机在姿态变化较大时出现控制数值不稳定甚至解算错误的问题。因此现有的技术方案难以绕开欧拉角,而在大姿态角度条件下,欧拉角的控制方案轻则出现求解数值不稳定,重则无法进行准确的控制。
技术实现思路
1、本发明提供一种四旋翼无人机的全姿态控制方法,旨在解决现有技术中解决过程高度依赖欧拉角,数值稳定性不佳以及大角度下解算错误的问题。
2、所述全姿态控制方法包括以下步骤:
3、s1、获取所述四旋翼无人机的目标姿态,对所述目标姿态进行预处理,得到预处理目标姿态;
4、s2、通过对所述预处理目标姿态进行计算,得到期望姿态;通过对所述期望姿态进行计算,得到所述四旋翼无人机的旋转方向和旋转角度;
5、s3、获取所述四旋翼无人机的角速度原始值;
6、s4、通过对所述角速度原始值进行计算,得到角速度增益系数;
7、s5、通过对所述角速度增益系数进行调整,得到所述四旋翼无人机的期望角速度;
8、s6、通过pid控制器跟踪所述期望角速度,得到目标力矩;
9、s7、根据所述目标力矩对所述四旋翼无人机的姿态进行控制。
10、优选的,步骤s1中,所述预处理目标姿态的计算公式如下:
11、
12、其中,qreal表示所述预处理目标姿态,表示四元数的乘法,qraw表示所述四旋翼无人机无姿态变化时的四元数姿态,q*表示四元数q的共轭值,qt表示所述四旋翼无人机姿态控制时的四元数姿态。
13、优选的,步骤s2中,所述旋转方向的计算公式如下:
14、qerr=[qe0 qe1 qe2 qe3];
15、v=[qe1 qe2 qe3]t=[vx vy vz]t;
16、其中,v表示所述旋转方向,vx、vy和vz分别表示旋转向量在x、y和z轴的分量大小,qerr表示所述期望姿态,qe0、qe1、qe2、qe3均表示所述期望姿态qerr的内部数值。
17、优选的,步骤s2中,所述旋转角度的计算公式如下:
18、
19、其中,α表示预设常数,θ表示所述旋转角度。
20、优选的,步骤s3中,所述角速度原始值的计算公式如下:
21、
22、其中,s表示常数,用于消除极点,ωraw表示所述角速度原始值,qx、qy、qz、qw均表示所述预处理目标姿态qreal的内部数值。
23、优选的,步骤s4中,得到所述角速度增益系数的计算公式如下:
24、
25、其中,k表示所述角速度增益系数,ωx、ωy、ωz分别表示。
26、优选的,步骤s5中,得到所述期望角速度的计算公式如下:
27、ω=kωraw;
28、其中,ω表示所述期望角速度。
29、优选的,步骤s6中,得到所述目标力矩的计算公式如下:
30、m=g(ω-ωnow);
31、其中,m表示所述目标力矩,g表示角速度跟踪方法,ωnow表示测量或估计得到的角速度。
32、与现有技术相比,本发明的有益效果在于:稳定性更好,不存在使用欧拉角时会出现的极点或数值漂移的问题,通过对数值进行的稳定,保证了控制时的稳定性;健壮性更强,可以适应所有姿态的刚体运行控制情形,无姿态控制的限制,对大多数的大角度机动和极点附件的运动,不会出现控制失效的问题;计算更快速且易于优化,相对于现有的欧拉角方案,三角函数等需要迭代的运算的使用频次低,也并非关键步骤,易于通过近似进行优化。
1.一种四旋翼无人机的全姿态控制方法,其特征在于,所述全姿态控制方法包括以下步骤:
2.如权利要求1所述的四旋翼无人机的全姿态控制方法,其特征在于,步骤s1中,所述预处理目标姿态的计算公式如下:
3.如权利要求2所述的四旋翼无人机的全姿态控制方法,其特征在于,步骤s2中,所述旋转方向的计算公式如下:
4.如权利要求3所述的四旋翼无人机的全姿态控制方法,其特征在于,步骤s2中,所述旋转角度的计算公式如下:
5.如权利要求4所述的四旋翼无人机的全姿态控制方法,其特征在于,步骤s3中,所述角速度原始值的计算公式如下:
6.如权利要求5所述的四旋翼无人机的全姿态控制方法,其特征在于,步骤s4中,得到所述角速度增益系数的计算公式如下:
7.如权利要求6所述的四旋翼无人机的全姿态控制方法,其特征在于,步骤s5中,得到所述期望角速度的计算公式如下:
8.如权利要求7所述的四旋翼无人机的全姿态控制方法,其特征在于,步骤s6中,得到所述目标力矩的计算公式如下: