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

说明书

本申请提供了一种小型无人机姿态与位置解算方法，其中，姿态解算包括：根据三轴加速度计和三轴磁强计推算出无人机在静止条件下的初始姿态角，接着根据三轴陀螺仪输出的角速度进行积分推算后续每一时刻的无人机姿态角；在无人机运动时同步计算无人机姿态角；最后，将上述过程的两个无人机姿态角分别进行高通滤波和低通滤波及融合，得到无人机姿态；位置解算包括：首先根据经纬度模块获得无人机的经度、纬度、高度信息，将其转换到以某一已知固定点为坐标原点的地面坐标系；之后，根据三轴加速度计结合上述姿态角进行二次积分得出相对于无人机运动起始点的三轴坐标；最后将上述过程的位置信息分别进行高通和低通滤波及融合，得到无人机的位置。

技术领域

本申请属于飞行器控制技术领域，特别涉及一种小型无人机姿态与位置解算方法。

背景技术

导航系统是小型无人机自主飞行过程中进行状态估计和环境感知必不可少的组成部分。其中，姿态和位置计算是其最主要的功能之一，同样也是自主飞行的关键。通常情况下，状态估计通过融合来自不同传感器的多源信息，进一步估计无人机姿态、高度、速度和位置。

传统导航系统通常依靠单一传感器的输出数据进行姿态和位置解算，未能根据传感器的工作特点充分有效利用惯性传感器及卫星定位系统，导致在较为恶劣的工作条件下受外界振动干扰影响较大，输出数据稳定性和精度较差，甚至数据发散而造成输出结果失效，严重时可造成无人机失控。

发明内容

本申请的目的是提供了一种小型无人机姿态与位置解算方法，以解决或减轻背景技术中的至少一个问题。

本申请的技术方案是：一种小型无人机姿态与位置解算方法，其中：

姿态解算包括：

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

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

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

位置解算包括：

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

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

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

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

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

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

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

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

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