[发明专利]基于双欧法和四元数混合编排的无人机导航方法和系统

说明书

本申请涉及一种基于双欧法和四元数混合编排的无人机导航方法和系统。所述方法包括：分别获取无人机水平飞行模式时对应的水平欧拉角表示的姿态旋转矩阵，以及垂直飞行模式时对应的垂直欧拉角表示的姿态旋转矩阵，当无人机的俯仰角大于设定的阈值时，将无人机的姿态旋转矩阵的表示形式切换为垂直欧拉角表示，当无人机的俯仰角小于或等于设定的阈值时，将无人机的姿态旋转矩阵的表示形式切换为水平欧拉角表示，在导航内部编排时，采用四元数表示对垂直欧拉角表示和水平欧拉角表示进行姿态更新，从而对无人机进行导航。采用本方法能够避免导航时姿态控制出现奇异值。

技术领域

本申请涉及无人机技术领域，特别是涉及一种基于双欧法和四元数混合编排的无人机导航方法和系统。

背景技术

随着无人机技术的发展，无人机也被用在生产生活中的各个领域中，从而对无人机的飞行形成有了新的要求。

常用的固定翼无人机，可以采用欧拉角表示方法，对导航时的姿态进行控制，然而，在无人机的俯仰角等于或者接近±90度时，导致计算结果出现奇异值，从而无法正常的执行飞行导航控制。

发明内容

基于此，有必要针对上述技术问题，提供一种能够解决无人机欧拉角表示法进行姿态控制时存在奇异值问题的基于双欧法的无人机导航方法和装置。

一种基于双欧法和四元数混合编排的无人机导航方法，所述方法包括：

分别获取无人机水平飞行模式时对应的水平欧拉角表示的姿态旋转矩阵，以及垂直飞行模式时对应的垂直欧拉角表示的姿态旋转矩阵；所述水平飞行模式为按传统固定翼飞机飞行的模式，无人机机体的纵轴成水平状态；所述垂直飞行模式为按传统四旋翼飞机飞行的模式，无人机机体的纵轴成竖直状态；

当无人机的俯仰角大于设定的阈值时，将无人机的姿态旋转矩阵的表示形式切换为垂直欧拉角表示；

当无人机的俯仰角小于或等于设定的阈值时，将无人机的姿态旋转矩阵的表示形式切换为水平欧拉角表示；

在导航内部编排时，采用四元数表示对所述垂直欧拉角表示和所述水平欧拉角表示进行姿态更新，从而对无人机进行导航。

在其中一个实施例中，还包括：获取当地地理坐标系对应于无人机载体坐标系的旋转矩阵；根据所述旋转矩阵计算当地重力加速度在无人机载体坐标系中的水平投影，得到所述六自由度运动模型的水平欧拉角表示，根据所述水平欧拉角表示的六自由度运动模型，得到水平欧拉角表示的姿态旋转矩阵。

在其中一个实施例中，还包括：获取当地地理坐标系对应于无人机载体坐标系的旋转矩阵；根据所述旋转矩阵计算当地重力加速度在无人机载体坐标系中的垂直投影，得到所述六自由度运动模型的垂直欧拉角表示，根据所述垂直欧拉角表示的六自由度运动模型，得到垂直欧拉角表示的姿态旋转矩阵。

在其中一个实施例中，还包括：根据所述水平欧拉角表示的六自由度运动模型，得到水平欧拉角表示的姿态旋转矩阵为：

其中，B表示姿态旋转矩阵，水平欧拉角φ绕X_b轴的旋转，水平欧拉角θ绕Y_b轴旋转，水平欧拉角ψ绕Z_b轴旋转，其中水平欧拉角的旋转顺序为Z_b-Y_b-X_b；C表示下标余弦值，S表示下标的正弦值。

在其中一个实施例中，还包括：根据所述垂直欧拉角表示的六自由度运动模型，得到垂直欧拉角表示的姿态旋转矩阵为：

其中，B表示姿态旋转矩阵，垂直欧拉角φ_v绕轴的旋转，垂直欧拉角θ_v绕轴旋转，垂直欧拉角ψ_v绕轴旋转，其中垂直欧拉角的旋转顺序为C表示下标余弦值，S表示下标的正弦值。