一种小型无人机姿态与位置解算方法技术

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

【技术实现步骤摘要】
一种小型无人机姿态与位置解算方法


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

技术介绍

[0002]导航系统是小型无人机自主飞行过程中进行状态估计和环境感知必不可少的组成部分。其中，姿态和位置计算是其最主要的功能之一，同样也是自主飞行的关键。通常情况下，状态估计通过融合来自不同传感器的多源信息，进一步估计无人机姿态、高度、速度和位置。
[0003]传统导航系统通常依靠单一传感器的输出数据进行姿态和位置解算，未能根据传感器的工作特点充分有效利用惯性传感器及卫星定位系统，导致在较为恶劣的工作条件下受外界振动干扰影响较大，输出数据稳定性和精度较差，甚至数据发散而造成输出结果失效，严重时可造成无人机失控。

技术实现思路

[0004]本申请的目的是提供了一种小型无人机姿态与位置解算方法，以解决或减轻
技术介绍
中的至少一个问题。
[0005]本申请的技术方案是：一种小型无人机姿态与位置解算方法，其中：
[0006]姿态解算包括：
[0007]根据三轴加速度计和三轴磁强计在无人机静止条件下的输出推算出无人机的初始姿态角，接着根据三轴陀螺仪输出角速度进行积分推算后续每一时刻的第一姿态角；
[0008]在无人机运动时，根据三轴加速度计和三轴磁强计输出同步计算第二姿态角；
[0009]将根据陀螺仪输出推算的第一姿态角与根据加速度计和磁强计输出推算的第二姿态角分别进行高通滤波和低通滤波，输入互补滤波器进行融合，最终得到无人机的姿态；r/>[0010]位置解算包括：
[0011]根据经纬度接收模块获得无人机的经度、纬度和高度信息，并将上述信息转换到以某已知固定点为坐标原点的地面坐标系，输出无人机在地面坐标系中的位置坐标信息；
[0012]根据最终得到的无人机姿态角进行二次积分得出相对于无人机运动起始点的三轴坐标；
[0013]最后将无人机地面坐标系转的位置信息和无人机姿态角积分得到的位置信息分别进行高通和低通滤波输入互补滤波器进行融合，最终得到无人机的位置。
[0014]在本申请中，根据三轴加速度计和三轴磁强计在无人机静止条件下的输出推算出无人机的初始姿态角的过程包括：
[0015]从地面坐标系到机体坐标系进行坐标矩阵转换：
[0016][0017]式中，为转换矩阵，θ、ψ、φ分别为俯仰角、偏航角和滚转角；
[0018]三轴加速度计的敏感轴沿机体坐标系各轴安装，在静基座初始对准时，其输出与地面坐标系中的比力关系为：
[0019][0020]式中，f
b
为加速度计三轴输出，n
x
、n
y
和n
z
为三轴加速度计测量到的无人机加速度在机体坐标系三轴方向上的分量与当地重力加速度之比，g为当地重力加速度；
[0021]三轴磁强计的各敏感轴沿无人机坐标系各轴安装，其测得三轴磁场强度和导航坐标系与无人机坐标系的关系为：
[0022]其中，
[0023]式中，M
b
为无人机坐标系下测得三轴磁场强度，M
n
为导航坐标系下测得三轴磁场强度，H0为初始高度；
[0024]当无人机静止时，三轴加速度计可以测量到重力加速度在无人机三轴上的投影，三轴磁力计的输出为地磁场在无人机三轴上的投影，得到计算辅助姿态角为：
[0025][0026]式中，n
x
、n
y
和n
z
为加速度计的三轴输出，H
x
、H
y
和H
z
为磁力计的三轴输出，θ、ψ为无人机的欧拉角。
[0027]在本申请中，根据三轴陀螺仪输出角速度进行积分推算后续每一时刻的第一姿态角为：
[0028][0029]式中，k为时刻，p、q、r为三轴陀螺仪测量到的当前时刻系统的角速度信息；Δt为三轴陀螺仪输出时间间隔。
[0030]在本申请中，根据最终得到的无人机姿态角进行二次积分得出相对于无人机运动起始点的三轴坐标的过程包括：
[0031]建立地面坐标系S
g
(o
g
，x
g
，y
g
，z
g
)，以一已知经纬高的地点为原点，将经纬度坐标转换为地面坐标系中的坐标为：
[0032][0033]进一步的可得，
[0034]式中，λ、分别为经纬度模块所在位置的经度和纬度，λ0、φ0分别为坐标原点的经度和纬度，R
m
为当地子午圈曲率半径，R
n
为当地卯酉圈曲率半径；
[0035]在经纬度模块输出数据并进行坐标转换的同时，使用惯性传感器进行二次积分估算无人机位置，再将二者融合输出高精度高更新速率的位置信息，
[0036][0037]式中，u、v和w为无人机速度投影到机体坐标系三轴方向上的分量，g为当地重力加速度，n
x
、n
y
和n
z
为三轴加速度计测量到的无人机加速度在机体坐标系三轴方向上的分量与当地重力加速度之比，p、q和r为三轴陀螺仪测量到的无人机角速度在机体坐标系三轴方向上的分量，φ和θ为无人机的滚转角和俯仰角。
[0038]在本申请中，最终得到无人机的位置为：
[0039][0040]式中，x，y，k为无人机的位置坐标及高度坐标。
[0041]本申请所提供的方法相比依靠单一传感器数据进行姿态与位置解算，本算法采用的基于惯性传感器和GPS/北斗模块的多传感器数据融合算法输出的姿态与位置信息提降低了输出噪声，提高了测量精度，提高了信息更新速率，提高了长期稳定性。
附图说明
[0042]为了更清楚地说明本申请提供的技术方案，下面将对附图作简单地介绍。显而易见地，下面描述的附图仅仅是本申请的一些实施例。
[0043]图1为本申请方法中姿态融合流程示意图。
[0044]图2为本申请方法中位置融合流程示意图。
具体实施方式
[0045]为使本申请实施的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行更加详细的描述。
[0046]为了解决
技术介绍
中所指出的问题，本申请提供一种姿态与位置的解算方法，根据不同传感器的工作特点将各自输出进行融合，使其能在占用较小计算资源的条件下尽可能提高解算的准确性，降低外界振动干扰对数据解算稳定性的影响。
[0047]本申请提供的小型无人机姿态与位置解算方法主要包括两部分：
[0048]1、姿态解算方法：
[0049]如图1所示，首先，根据三轴加速度计和三轴磁强计在无人机静止条件下的输出推算出无人机的初始姿态角，接着根据三轴陀螺仪输出的角速度进行积分推算后续每一时刻的无人机姿态角；
[0050]之后，在无人机运动时根据三轴加速度本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种小型无人机姿态与位置解算方法，其特征在于，姿态解算包括：根据三轴加速度计和三轴磁强计在无人机静止条件下的输出推算出无人机的初始姿态角，接着根据三轴陀螺仪输出角速度进行积分推算后续每一时刻的第一姿态角；在无人机运动时，根据三轴加速度计和三轴磁强计输出同步计算第二姿态角；将根据陀螺仪输出推算的第一姿态角与根据加速度计和磁强计输出推算的第二姿态角分别进行高通滤波和低通滤波，输入互补滤波器进行融合，最终得到无人机的姿态；位置解算包括：根据经纬度接收模块获得无人机的经度、纬度和高度信息，并将上述信息转换到以某已知固定点为坐标原点的地面坐标系，输出无人机在地面坐标系中的位置坐标信息；根据最终得到的无人机姿态角进行二次积分得出相对于无人机运动起始点的三轴坐标；最后将无人机地面坐标系转的位置信息和无人机姿态角积分得到的位置信息分别进行高通和低通滤波输入互补滤波器进行融合，最终得到无人机的位置。2.如权利要求1所述的小型无人机姿态与位置解算方法，其特征在于，根据三轴加速度计和三轴磁强计在无人机静止条件下的输出推算出无人机的初始姿态角的过程包括：从地面坐标系到机体坐标系进行坐标矩阵转换：式中，为转换矩阵，θ、ψ、φ分别为俯仰角、偏航角和滚转角；三轴加速度计的敏感轴沿机体坐标系各轴安装，在静基座初始对准时，其输出与地面坐标系中的比力关系为：式中，f
b
为加速度计三轴输出，n
x
、n
y
和n
z
为三轴加速度计测量到的无人机加速度在机体坐标系三轴方向上的分量与当地重力加速度之比，g为当地重力加速度；三轴磁强计的各敏感轴沿无人机坐标系各轴安装，其测得三轴磁场强度和导航坐标系与无人机坐标系的关系为：其中，式中，M
b
为无人机坐标系下测得三轴磁场强度，M
n
为导航坐标系下测得三轴磁场强度，H0为初始高度；
当无人机静止时，三轴加速度计可以测量到重力加速度在无人机三轴上的投影，三轴磁力计的输出为地磁场在无人机三轴上的投影，得到计算辅助姿态角为：式中，n
x
、n
y
和n
z

【专利技术属性】
技术研发人员：赵晋伟，王志刚，杨大鹏，
申请(专利权)人：中国航空工业集团公司沈阳飞机设计研究所，
类型：发明
国别省市：