[发明专利]一种小型无人机姿态与位置解算方法

权利要求书

1.一种小型无人机姿态与位置解算方法，其特征在于，

姿态解算包括：

根据三轴加速度计和三轴磁强计在无人机静止条件下的输出推算出无人机的初始姿态角，接着根据三轴陀螺仪输出角速度进行积分推算后续每一时刻的第一姿态角；

在无人机运动时，根据三轴加速度计和三轴磁强计输出同步计算第二姿态角；

将根据陀螺仪输出推算的第一姿态角与根据加速度计和磁强计输出推算的第二姿态角分别进行高通滤波和低通滤波，输入互补滤波器进行融合，最终得到无人机的姿态；

所述根据三轴加速度计和三轴磁强计在无人机静止条件下的输出推算出无人机的初始姿态角的过程包括：

从地面坐标系到机体坐标系进行坐标矩阵转换：

式中，为转换矩阵，θ、ψ、φ分别为俯仰角、偏航角和滚转角；

三轴加速度计的敏感轴沿机体坐标系各轴安装，在静基座初始对准时，其输出与地面坐标系中的比力关系如下：

式中，f_b为加速度计三轴输出，n_x、n_y和n_z为三轴加速度计测量到的无人机加速度在机体坐标系三轴方向上的分量与当地重力加速度之比，g为当地重力加速度；

三轴磁强计的各敏感轴沿无人机坐标系各轴安装，其测得三轴磁场强度和导航坐标系与无人机坐标系的关系如下：

其中，

式中，M_b为无人机坐标系下测得三轴磁场强度，M_n为导航坐标系下测得三轴磁场强度，H₀为初始高度；

当无人机静止时，三轴加速度计测量到重力加速度在无人机三轴上的投影，三轴磁力计的输出为地磁场在无人机三轴上的投影，得到计算辅助姿态角：

式中，H_x、H_y和H_z为磁力计的三轴输出；

根据三轴陀螺仪输出角速度进行积分推算后续每一时刻的第一姿态角为：

其中，k为时刻，p_k、q_k、r_k为三轴陀螺仪测量到的当前时刻系统的角速度信息，Δt为三轴陀螺仪输出时间间隔；

位置解算包括：

根据经纬度接收模块获得无人机的经度、纬度和高度信息，并将上述信息转换到以某已知固定点为坐标原点的地面坐标系，输出无人机在地面坐标系中的位置坐标信息；

根据最终得到的无人机姿态角进行二次积分得出相对于无人机运动起始点的三轴坐标；

最后将无人机地面坐标系转的位置信息和无人机姿态角积分得到的位置信息分别进行高通和低通滤波输入互补滤波器进行融合，最终得到无人机的位置；

根据最终得到的无人机姿态角进行二次积分得出相对于无人机运动起始点的三轴坐标的过程包括：

建立地面坐标系S_g(o_gx_gy_gz_g)，原点选在一已知经纬高的地点，将经纬度坐标转换为地面坐标系中的坐标：

式中，λ、分别为经纬度模块所在位置的经度和纬度，λ₀、分别为坐标原点的经度和纬度，R_m为当地子午圈曲率半径，R_n为当地卯酉圈曲率半径；

进一步可得：

在接收经纬度模块输出数据并进行坐标转换的同时，使用惯性传感器进行二次积分估算无人机位置，再将二者融合输出高精度高更新速率的位置信息：

式中，u、v和w为无人机速度投影到机体坐标系三轴方向上的分量，g为当地重力加速度，n_x、n_y和n_z为三轴加速度计测量到的无人机加速度在机体坐标系三轴方向上的分量与当地重力加速度之比，p、q和r为三轴陀螺仪测量到的无人机角速度在机体坐标系三轴方向上的分量，φ和θ为无人机的滚转角和俯仰角；

最终得到无人机的位置为：

式中，x，y，h为无人机的位置坐标及高度坐标。