CN112649001B

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

Info

Publication number: CN112649001B
Application number: CN202011382997.3A
Authority: CN
Inventors: 赵晋伟; 王志刚; 杨大鹏
Original assignee: Shenyang Aircraft Design and Research Institute Aviation Industry of China AVIC
Current assignee: Shenyang Aircraft Design and Research Institute Aviation Industry of China AVIC
Priority date: 2020-12-01
Filing date: 2020-12-01
Publication date: 2023-08-22
Anticipated expiration: 2040-12-01
Also published as: CN112649001A

Abstract

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

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

姿态解算包括：

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

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

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

位置解算包括：

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

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

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

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

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

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

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

式中，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_g，x_g，y_g，z_g)，以一已知经纬高的地点为原点，将经纬度坐标转换为地面坐标系中的坐标为：

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

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

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

在本申请中，最终得到无人机的位置为：

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

本申请所提供的方法相比依靠单一传感器数据进行姿态与位置解算，本算法采用的基于惯性传感器和GPS/北斗模块的多传感器数据融合算法输出的姿态与位置信息提降低了输出噪声，提高了测量精度，提高了信息更新速率，提高了长期稳定性。

附图说明

为了更清楚地说明本申请提供的技术方案，下面将对附图作简单地介绍。显而易见地，下面描述的附图仅仅是本申请的一些实施例。

图1为本申请方法中姿态融合流程示意图。

图2为本申请方法中位置融合流程示意图。

具体实施方式

为使本申请实施的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行更加详细的描述。

为了解决背景技术中所指出的问题，本申请提供一种姿态与位置的解算方法，根据不同传感器的工作特点将各自输出进行融合，使其能在占用较小计算资源的条件下尽可能提高解算的准确性，降低外界振动干扰对数据解算稳定性的影响。

本申请提供的小型无人机姿态与位置解算方法主要包括两部分：

1、姿态解算方法：

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

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

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

具体过程包括：

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

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

式中，f_b为加速度计三轴输出，g为当地重力加速度。

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

其中，

式中，M_b为无人机坐标系下测得三轴磁场强度，M_n为导航坐标系下测得三轴磁场强度。

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

式中，n_x、n_y和n_z为加速度计的三轴输出，H_x、H_y和H_z为磁力计的三轴输出，θ、φ、ψ为无人机的欧拉角。

通过上述算法获得初始姿态信息。然后根据三轴陀螺仪输出的角速度信息，可以按照下列公式推算姿态角：

其中，p、q、r为三轴陀螺仪测量到的当前时刻系统的角速度信息。Δt为三轴陀螺仪输出时间间隔，在本系统硬件平台中姿态更新时间间隔为3ms。

采用静止时三轴加速度计输出的重力加速度和三轴磁力计在无人机三轴方向上的投影来估计初始姿态角，接着使用三轴陀螺仪输出的无人机角速度在无人机三轴方向上的投影进行积分推算姿态角。然后将由三轴加速度计和三轴磁力计计算的辅助姿态角与由陀螺仪积分得到的推算姿态角分别进行低通滤波和高通滤波，输入互补滤波器中进行融合，完成基于惯性传感器的姿态估计。

2、位置解算方法：

如图2所示，首先根据经纬度模块获得无人机的经度、纬度、高度信息(例如可以采用GNSS接收模块收到的北斗或GPS导航报文，经过筛选后得到经度、纬度、高度信息)，接着将其转换到以某一已知固定点为坐标原点的地面坐标系，输出无人机在地面坐标系中的坐标；

之后，根据三轴加速度计输出结合上述姿态计算方法输出的姿态角进行二次积分得出相对于无人机运动起始点的三轴坐标；

最后将第一步和第二步输出的位置信息分别进行高通和低通滤波输入互补滤波器进行融合，最终得到无人机的位置。

具体过程包括：

位置解算首先将所需的经纬高信息从导航报文中筛选出来，然后建立地面坐标系S_g(o_gx_gy_gz_g)，原点选在某一已知经纬高的地点，o_gx_g指北，o_gy_g指东。按照下述公式即可将经纬度坐标转换为地面坐标系中的坐标，单位为米。

在该公式中，λ表示经度，φ表示纬度，R_m为当地子午圈曲率半径，R_n为当地卯酉圈曲率半径。二者具体下列公式计算。

在上述公式中，取地球半径R_e＝6378137m，地球扁率e＝0.0033528131779，(λ₀,φ₀)为坐标原点O的经度和纬度，(λ,φ)为GPS/北斗模块接收天线所在位置的经度和纬度。

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

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

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

最后，本申请中还提供了一种计算机设备，包括：一个或多个处理器；存储装置，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行时，使得一个或多个处理器实现如上述中任一项所述的方法。

本申请实施例还提供了一种计算机可读存储介质，其存储有计算机程序，其存储有计算机程序，该程序被处理器执行时实现如上述中任一项所述的方法。

应当理解，本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤、方法、装置或模块可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以所述权利要求的保护范围为准。

Claims

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

姿态解算包括：

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

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

其中，

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

式中，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为无人机的位置坐标及高度坐标。

2.一种计算机设备，其特征在于，包括：

一个或多个处理器；

存储装置，用于存储一个或多个程序；

当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现如权利要求1所述的方法。

3.一种计算机可读存储介质，其存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1所述的方法。