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

权利要求书

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

获取当地地理坐标系对应于无人机载体坐标系的旋转矩阵；根据所述旋转矩阵计算当地重力加速度在无人机载体坐标系中的水平投影，得到六自由度运动模型中水平欧拉角表示，根据所述水平欧拉角表示的六自由度运动模型，得到水平欧拉角表示的姿态旋转矩阵，根据所述旋转矩阵计算当地重力加速度在无人机载体坐标系中的垂直投影，得到六自由度运动模型的垂直欧拉角表示，根据所述垂直欧拉角表示的六自由度运动模型，得到垂直欧拉角表示的姿态旋转矩阵；所述水平飞行模式为按传统固定翼飞机飞行的模式，无人机机体的纵轴成水平状态；所述垂直飞行模式为按传统四旋翼飞机飞行的模式，无人机机体的纵轴成竖直状态；

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

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

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

2.根据权利要求1所述的方法，其特征在于，所述根据水平欧拉角表示的六自由度运动模型，得到水平欧拉角表示的姿态旋转矩阵，包括：

根据所述水平欧拉角表示的六自由度运动模型，得到水平欧拉角表示的姿态旋转矩阵为：

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

3.根据权利要求1所述的方法，其特征在于，根据垂直欧拉角表示的六自由度运动模型，得到垂直欧拉角表示的姿态旋转矩阵，包括：

根据所述垂直欧拉角表示的六自由度运动模型，得到垂直欧拉角表示的姿态旋转矩阵为：

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

4.根据权利要求3所述的方法，其特征在于，在导航内部编排时，采用四元数表示对所述垂直欧拉角表示进行姿态更新，包括：

在导航内部编排时，获取姿态四元数表示；

根据所述姿态四元数表示，确定四元数更新方程；

根据所述四元数更新方程，得到姿态旋转矩阵的四元数表示；

根据所述四元数表达，确定垂直欧拉角表示的姿态更新参数。

5.根据权利要求4所述的方法，其特征在于，在导航内部编排时，采用四元数表示对所述水平欧拉角表示进行姿态更新，包括：

在导航内部编排时，获取姿态四元数表示；

根据所述姿态四元数表示，确定四元数更新方程；

根据所述四元数更新方程，得到姿态旋转矩阵的四元数表示；

根据所述四元数表示，确定水平欧拉角表示的姿态更新参数。