CN105021183A - 多旋翼飞行器gps和ins低成本组合导航系统 - Google Patents

Abstract

本发明公开了多旋翼飞行器GPS和INS低成本组合导航系统，包括数据采集处理器，数据采集处理器通过IIC总线分别连接陀螺仪、加速度计、磁力计、气压计，数据采集处理器控制陀螺仪、加速度计、磁力计、气压计采集数据，并将采集到的传感器数据发送给DSP控制器，GPS模块将定位数据发送给DSP控制器，DSP控制器将传感器数据和定位数据进行导航解算。本发明的有益效果是：根据多旋翼飞行器的动态特性及低成本传感器的误差特点，为飞行器提供准确的姿态、速度和位置信息。

Description

多旋翼飞行器GPS和INS低成本组合导航系统

技术领域

本发明属于组合导航技术领域，涉及基于GPS和INS松耦合方式的多旋翼飞行器低成本组合导航系统。

背景技术

在已有的多种导航技术中，惯性导航、卫星导航以及二者的组合导航在当前研究和应用中最为广泛。惯性导航系统(INS)以经典物理学理论为基础，利用惯性传感器和导航计算机推算用户的姿态角度、速度和位置，是一种三维航位推算(DR)导航系统。INS的优点在于其高带宽的导航结果，具有短期精度，缺点是INS的误差会随时间累积，精度较高的INS价格也非常昂贵。全球导航卫星系统(GNSS)是与INS的航位推算原理根本区别的一种直接定位系统，它测量用户相对导航卫星构成星座的坐标，再由卫星运行轨道计算卫星相对地球的坐标，最后计算出用户在地球上的位置。GNSS的误差不随时间累积，具有长期精度，但易受干扰，工作频率较低。根据INS和GNSS的互补特性，将GNSS和INS组合起来，可以实现超越二者单独工作的导航性能。

然而，精度较高的INS成本极高，低成本的惯性传感器由于噪声较大，漂移严重，在实际使用中面临极大挑战。低成本的单点GNSS接收机通常工作频率极低(<5Hz)，无法获取用户的姿态信息，对于飞行器等对姿态要求很高的系统而言无法单独使用，而差分GNSS接收机、多天线接收机等通常成本也很高，对大多数民用系统而言很难承受。因此，结合低成本INS和低成本GNSS对于要求姿态、低成本和实时性的民用导航用户而言几乎是唯一的选择。低成本组合导航系统也已成为国内外学术研究和产品化的热点，得到了科研单位和导航企业的极大重视，具有丰富的学术研究价值和市场价值。目前，多旋翼飞行器已经被广泛应用于许多领域，如军事、交通和物流等。作为实现其自主任务飞行的关键技术之一，多旋翼飞行器的低成本导航已成为国内外研究的热点。单纯的低成本GPS无法提供飞行器的姿态信息，输出频率低且精度易受环境因素影响，而单纯的低成本INS则噪声较大，漂移严重，因此，将二者有效地进行数据融合和优势互补是低成本导航应用的必然选择。

多旋翼飞行器通常具有低动态特性，但在飞行过程中，电机转动引起的机身振动会极大降低导航系统性能。大量研究表明，多旋翼飞行器的振动对导航系统的陀螺仪、加速度计、磁力计和气压计均会有不容忽略的影响。因此，需要针对性的对多旋翼飞行器设计有效的滤波算法。

发明内容

本发明的目的在于解决现有的导航系统精度差或成本高的问题。

本发明所采用的技术方案是包括数据采集处理器，数据采集处理器通过IIC总线分别连接陀螺仪、加速度计、磁力计、气压计，数据采集处理器控制陀螺仪、加速度计、磁力计、气压计采集数据，并将采集到的传感器数据发送给DSP控制器，GPS模块将定位数据发送给DSP控制器，DSP控制器将传感器数据和定位数据进行导航解算。

进一步，所述气压计的采样频率为10Hz，所述陀螺仪、加速度计、磁力计的采样频率均为50Hz。

进一步，所述陀螺仪型号为L3G4200D，加速度计型号为ADXL345，磁力计型号为HMC5883L，气压计型号为BMP085，GPS模块型号为Ublox NEO-6M，数据采集处理器型号为STM32F103VET6，DSP控制器型号为TMS320F28335。

GPS和INS松耦合方式多旋翼飞行器组合导航系统进行导航的方法，按照以下步骤进行：

步骤1：预处理；

a)低成本组合导航系统安装到飞行器，Y轴指向飞行器机身的正前方，X轴指向机身的右方，Z轴指向机身的正上方；

b)离地1米以上固定飞行器，保证飞行器不会起飞；

c)从静止时刻开始，间隔1min增大电调的PWM控制信号高电平持续时间0.02ms，并实时采集并保存陀螺仪、加速度计、磁力计和气压计的数据；

d)分别对磁力计和气压计估计每个PWM控制信号变化的时间间隔内采集的数据的方差，并作图，纵坐标为方差数据，横坐标为对应的PWM高电平持续时间，使用MATLAB的cftool工具箱对每条曲线进行拟合，得到磁力计和气压计每个轴的数据方差与控制信号之间的关系模型MC1和MC2。

组合导航系统加电启动，点亮LED状态灯，数据采集处理器以50Hz频率通过IIC总线读取陀螺仪、加速度计和磁力计的数据，得到飞行器当前的三轴角速率(w_x、w_y、w_z)、三轴加速度(a_x、a_y、a_z)和三轴磁场强度(m_x、m_y、m_z)；

步骤2：重复步骤1并存储100组三轴角速率和三轴加速度，检测判断是否为飞行器静止状态采样的数据；

步骤3：若步骤2中的静止状态检测不通过，重复步骤1～步骤2，直到满足条件1和条件2同时成立条件为止；

步骤4：步骤2的静止状态检测通过后，估计陀螺仪的零偏，零偏估计完成后，点亮LED状态灯；

步骤5：在10s时间内，手动将组合导航系统绕其X、Y、Z轴旋转一次，计算磁力计零偏，计算完成后，点亮LED状态灯；

步骤6：数据采集处理器以10Hz读取气压计采样数据，连续读取10组，得到初始状态初始气压高(Baro_InitHeight)；

步骤7：DSP控制器通过串口USART3接收当前的GPS定位数据：λ_GPS(经度)、L_GPS(纬度)、H_GPS(高度)、VE_GPS(东向速度)、VN_GPS(北向速度)、VU_GPS(天向速度)；

步骤8：重复步骤7接收30次有效GPS数据，将最新的GPS定位数据发送至DSP控制器；

步骤9：DSP控制器修正陀螺仪和磁力计的零偏，将气压高修正到与GPS高同步，得到当前的气压高采样；

步骤10：分别以50HZ采集陀螺仪、加速度计和磁力计数据，以10Hz采集气压高度计数据，每一次采样数据经过步骤9处理；

步骤11：对陀螺仪和加速度计数据做前置低通滤波；

步骤12：计算惯性导航系统的粗对准初始参数；

步骤13：重复步骤9和步骤10直到任务完成为止，更新姿态、速度和位置；

步骤14：DSP控制器发送当前解算的姿态、速度、位置以及采集的传感器数据，包括三轴加速度、三轴角速度、三轴磁场强度和气压高，以及解算的当前姿态角观测量和γ(k)，发送完成后，惯导计算机修正姿态、速度和位置误差；

步骤15：DSP控制器接收数据采集处理器的数据，并始终只保留最新一次解算的结果，接收GPS定位数据，每一次接收完成后，对GPS做前置滤波；

步骤16：组合导航计算机利用惯导计算机发送的数据和步骤15处理后的数据，计算惯性导航系统的修正量；

步骤17：将步骤16的组合导航结果发送至惯导计算机并用步骤14的方法修正惯导参数，发送完成后重复执行步骤15-16；

步骤18：惯导计算机判断，接收到的三个角度修正量的变化均小于1度时，点亮LED，表示初始对准完成，并通知飞控系统可以起飞，同时将当前的姿态角、速度和位置信息发送至飞控系统。飞行系统收到后将当前的最大控制脉宽发送至导航系统，导航系统根据预处理建立的MC1和MC2模型分别计算磁航向角、气压高度的方差估计值；

步骤19：重复执行步骤17和步骤13，即可实现完整的多旋翼飞行器导航应用。

本发明的有益效果是：根据多旋翼飞行器的动态特性及低成本传感器的误差特点，为飞行器提供准确的姿态、速度和位置信息。

附图说明

图1是本发明硬件系统示意图；

图2是本发明导航方法流程示意图；

图3是本发明STM32程序处理流程图；

图4是本发明DSP程序处理流程图。

图中，1.数据采集处理器，2.陀螺仪，3.加速度计，4.磁力计，5.气压计，6.DSP控制器，7.GPS模块。

具体实施方式

下面结合具体实施方式对本发明进行详细说明。

本发明组合导航系统针对多旋翼飞行器而设计，多旋翼飞行器最常见的是四旋翼飞行器(Quad-Rotor)。四旋翼飞行器也叫四轴飞行器，是一种垂直起降(VTOL)的无人驾驶航空器，其衍生产品还包括六轴、八轴甚至十六轴等多旋翼飞行器。这种飞行器通常由机械结构、飞行控制及导航系统、动力系统、通信系统和任务模块组成，可以实现遥控或自主飞行。相比于固定翼无人机，四轴飞行器拥有旋翼无人机灵活、可悬停的优势；而相比传统的旋翼无人机，四轴飞行器具有更大的载重、更简单的机械结构和更灵活的飞行方式。

硬件系统如图1所示：数据采集处理器1控制陀螺仪2、加速度计3、磁力计4、气压计5采集数据，并将采集到的传感器数据发送给DSP控制器6，GPS模块7将定位数据发送给DSP控制器6，DSP控制器6将传感器数据和定位数据进行导航解算，气压计5的采样频率为10Hz，其余三个传感器的采样频率为50Hz。

其中，数据采集处理器1的型号为STM32，还连接有SD卡，DSP控制器6连接有SRAM。所述气压计5的采样频率为10Hz，所述陀螺仪2、加速度计3、磁力计4的采样频率均为50Hz。所述数据采集处理器1的型号为STM32，所述陀螺仪2型号为L3G4200D，加速度计3型号为ADXL345，磁力计4型号为HMC5883L，气压计5型号为BMP085，GPS模块7型号为Ublox NEO-6M，数据采集处理器1型号为STM32F103VET6，DSP控制器6型号为TMS320F28335。

由硬件系统示意图1，DSP控制器6是本发明的组合导航计算单元，称为组合导航计算机；数据采集处理器1即STM32为本发明的捷联式惯性导航系统计算单元，称为惯导计算机。传感器组由一个三轴加速度计、三轴陀螺仪、三轴磁力计、气压高度计和GPS接收机组成，均为低成本传感器。

加速度计、陀螺仪、磁力计和气压高度计连接至惯导计算机的IIC1，惯导计算机分别通过USART接口与飞行器的飞行控制系统和组合导航计算机交互数据。此外，惯导计算机还通过SDIO接口连接有SD卡。组合导航计算机通过串口与GPS接收机连接，同时，GPS接收机的PPS输出连接至组合导航计算机的外部中断触发引脚。同时，组合导航计算机还连接有一个SRAM。为了表征系统运行状态，惯导计算机连接有5个LED灯。

本发明GPS和INS松耦合方式多旋翼飞行器组合导航系统进行组合导航的方法为如图2至图4所示，图2是本发明导航方法流程示意图；图3是本发明STM32程序处理流程图；图4是本发明DSP程序处理流程图。详细方法步骤如下：

步骤1：预处理；

a)低成本组合导航系统安装到飞行器，Y轴指向飞行器机身的正前方，X轴指向机身的右方，Z轴指向机身的正上方；

b)离地1米以上固定飞行器，保证飞行器不会起飞；

c)从静止时刻开始，间隔1min增大电调的PWM控制信号高电平持续时间0.02ms，并实时采集并保存陀螺仪、加速度计、磁力计和气压计的数据；

d)分别对磁力计和气压计估计每个PWM控制信号变化的时间间隔内采集的数据的方差，并作图，纵坐标为方差数据，横坐标为对应的PWM高电平持续时间，使用MATLAB的cftool工具箱对每条曲线进行拟合，得到磁力计和气压计每个轴的数据方差与控制信号之间的关系模型MC1和MC2；

组合导航系统加电启动，点亮LED状态灯1，数据采集处理器1以50Hz频率通过IIC总线读取陀螺仪2、加速度计3和磁力计4的数据，得到飞行器当前的三轴角速率(w_x、w_y、w_z)、三轴加速度(a_x、a_y、a_z)和三轴磁场强度(m_x、m_y、m_z)；

步骤2：重复步骤1并存储100组三轴角速率和三轴加速度，按如下方法判断是否为飞行器静止状态采样的数据：

条件1： $T_a=\frac{1}{N}\underset{i=1}{\overset{N}{\&Sigma;}}\sqrt{a_x{\left(i\right)}^2+a_y{\left(i\right)}^2+a_z{\left(i\right)}^2}<{\mathrm{\&lambda;}}_a$

条件2： $T_g=\frac{1}{N}\underset{i=1}{\overset{N}{\&Sigma;}}\sqrt{w_x{\left(i\right)}^2+w_y{\left(i\right)}^2+w_z{\left(i\right)}^2}<{\mathrm{\&lambda;}}_g$

其中，λ_a＝1.1，λ_g＝3.5，N＝100。当且仅当条件1和条件2同时成立时，认为是静止采样数据；

步骤3：若步骤2中的静止状态检测不通过，即不满足任一条件时，重复步骤1～步骤2，直到满足条件1和条件2同时成立条件为止；

步骤4：步骤2的静止状态检测通过后，用下述方法估计陀螺仪2的零偏，零偏估计完成后，点亮LED状态灯2。

$gyro\_b_x=0.01*\underset{i=1}{\overset{100}{\&Sigma;}}{w}_x\left(i\right)$

$gyro\_b_y=0.01*\underset{i=1}{\overset{100}{\&Sigma;}}{w}_y\left(i\right)$

$gyro\_b_z=0.01*\underset{i=1}{\overset{100}{\&Sigma;}}{w}_z\left(i\right)$

步骤5：在10s时间内，手动将组合导航系统绕其X、Y、Z轴旋转一次，用下述方法计算磁力计4零偏[引用文献1]。计算完成后，点亮LED状态灯3。

L＝[-m_x(1)²-m_x(2)²...-m_x(n)²]^T，

$A=\left[\begin{array}{cccccc}-2m_x\left(1\right)& m_y{\left(1\right)}^2& -2m_y\left(1\right)& m_z{\left(1\right)}^2& -2m_z\left(1\right)& 1\\ -2m_x\left(2\right)& m_y{\left(2\right)}^2& -2m_y\left(2\right)& m_z{\left(2\right)}^2& -2m_z\left(2\right)& 1\\ .& .& .& .& .& .\\ .& .& .& .& .& .\\ .& .& .& .& .& .\\ -2m_x\left(n\right)& m_y{\left(n\right)}^2& -2m_y\left(n\right)& m_z{\left(n\right)}^2& -2m_z\left(n\right)& 1\end{array}\right],$

其中，n为采样数据数量。

令X＝(A^TA)^-1A^TL，

magn_b_x＝X(1)

magn_b_y＝X(3)/X(2)

magn_b_z＝X(5)/X(4)

X(1)-X(5)表示由磁力计三轴零偏参数构成的待求向量。

步骤6：数据采集处理器1以10Hz读取气压计5采样数据，连续读取10组，得到初始状态初始气压高(Baro_InitHeight)；

步骤7：DSP控制器6通过串口USART3接收当前的GPS定位数据：λ_GPS(经度)、L_GPS(纬度)、H_GPS(高度)、VE_GPS(东向速度)、VN_GPS(北向速度)、VU_GPS(天向速度)。

步骤8：重复步骤7接收30次有效GPS数据，将最新的GPS定位数据发送至DSP控制器6；

步骤9：DSP控制器6修正陀螺仪2和磁力计4的零偏，将气压高修正到与GPS高同步，得到当前的气压高采样，具体方法是：

w_axis(k)＝w_axis(k)-gyro_b_axis,axis＝x,y,z

m_axis(k)＝m_axis(k)-magn_b_axis,axis＝x,y,z

Baro_Height(k)＝Baro_Height(k)-(Baro_InitHeight-H_GPS(1))

步骤10：分别以50HZ采集陀螺仪、加速度计和磁力计数据，以10Hz采集气压高度计数据，每一次采样数据经过步骤9处理；

步骤11：对陀螺仪2和加速度计4数据做前置低通滤波。具体方法是：

$w_{axis}\left(k\right)=\underset{i=1}{\overset{5}{\&Sigma;}}f\left(i\right)\&CenterDot;w(k-i+1)+\underset{i=6}{\overset{10}{\&Sigma;}}f(11-i)\&CenterDot;w(k-i+1)$

$a_{axis}\left(k\right)=\underset{i=1}{\overset{5}{\&Sigma;}}f\left(i\right)\&CenterDot;a(k-i+1)+\underset{i=6}{\overset{10}{\&Sigma;}}f(11-i)\&CenterDot;a(k-i+1)$

这里，f＝[1.61e-2 3.79e-2 9.31e-2 1.55e-1 1.97e-1]。

k表示k时刻，f(i)表示f向量的第i个参数，这一步得到的是k时刻低通滤波后的三轴角速率和三轴线加速度。

步骤12：计算惯性导航系统的粗对准初始参数，具体方法是：

初始姿态角：

$\left\{\begin{array}{c}\mathrm{\&theta;}\left(1\right)=-a\sin \left(a_x\right)\\ \mathrm{\&phi;}\left(1\right)=a\tan 2(-a_{y:}-a_z)\\ \left[\begin{array}{c}{m}_x^{\&prime;}\\ m_y^{\&prime;}\\ m_z^{\&prime;}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\&phi;}& \sin \mathrm{\&theta;}\sin \mathrm{\&phi;}& -\cos \mathrm{\&theta;}\sin \mathrm{\&phi;}\\ 0& \cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\\ \sin \mathrm{\&phi;}& -\sin \mathrm{\&theta;}\cos \mathrm{\&phi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&phi;}\end{array}\right]\&CenterDot;\left[\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right]\\ \mathrm{\&psi;}\left(1\right)=a\tan 2\left(m_{x:}^{\&prime;}{m}_y^{\&prime;}\right)+\mathrm{\&Delta;}\mathrm{\&Psi;}\end{array}\right.$ ............公式(1)

这里，△Ψ表示当地的磁偏角，θ表示俯仰角，φ表示横滚角，ψ表示航向角。参数括号中的(1)表示第1个时刻数据，也就是初始值。m′_x、m′_y、m_z′表示飞行器的右、前、上方的磁场强度。

初始四元数：

$q\left(1\right)=\left[\begin{array}{c}{q}_1\left(1\right)\\ q_2\left(1\right)\\ q_3\left(1\right)\\ q_4\left(1\right)\end{array}\right]=\left[\begin{array}{c}cos\mathrm{\&psi;}\left(1\right)cos\mathrm{\&theta;}\left(1\right)cos\mathrm{\&phi;}\left(1\right)-sin\mathrm{\&psi;}\left(1\right)sin\mathrm{\&theta;}\left(1\right)sin\mathrm{\&phi;}\left(1\right)\\ \cos \mathrm{\&psi;}\left(1\right)sin\mathrm{\&theta;}\left(1\right)cos\mathrm{\&phi;}\left(1\right)-sin\mathrm{\&psi;}\left(1\right)cos\mathrm{\&theta;}\left(1\right)sin\mathrm{\&phi;}\left(1\right)\\ cos\mathrm{\&psi;}\left(1\right)\cos \mathrm{\&theta;}\left(1\right)sin\mathrm{\&phi;}\left(1\right)+sin\mathrm{\&psi;}\left(1\right)sin\mathrm{\&theta;}\left(1\right)cos\mathrm{\&phi;}\left(1\right)\\ \cos \mathrm{\&psi;}\left(1\right)sin\mathrm{\&theta;}\left(1\right)\sin \mathrm{\&phi;}\left(1\right)+sin\mathrm{\&psi;}\left(1\right)\cos \mathrm{\&theta;}\left(1\right)cos\mathrm{\&phi;}\left(1\right)\end{array}\right]$ …………公式(2)

公式2表示的是初始四元数，四元数是飞行器姿态角的一种表示方法。

初始速度：VE_INS(1)＝0(东向速度)，VN_INS(1)＝0(北向速度)，VU_INS(1)＝0(天向速度)。

初始位置：λ_INS(1)＝λ_GPS(1)(λ_INS为惯导系统计算的经度，λ_GPS为GPS接收机输出的经度)，L_INS(1)＝L_GPS(1)(L_INS为惯性导航系统计算的纬度，L_GPS为GPS接收机输出的纬度)，H_INS(1)＝H_GPS(1)(H_INS为惯性导航系统计算的海拔高，H_GPS为GPS接收机输出的海拔高)。

步骤13：重复步骤9和步骤10直到任务完成为止，更新姿态、速度和位置。

具体方法是：

姿态更新：

$q\left(k\right)=q(k-1)+0.01\left[\begin{array}{cccc}0& -w_x\left(k\right)& -w_y\left(k\right)& -w_z\left(k\right)\\ w_x\left(k\right)& 0& w_z\left(k\right)& -w_y\left(k\right)\\ w_y\left(k\right)& -w_z\left(k\right)& 0& w_x\left(k\right)\\ w_z\left(k\right)& w_y\left(k\right)& -w_x\left(k\right)& 0\end{array}\right]\&CenterDot;q(k-1)$ ……公式(3)

$q\left(k\right)=\frac{q\left(k\right)}{\left|\right|q\left(k\right)|{|}_2}$ ……公式(4)

$\mathrm{\&theta;}\left(k\right)=asin\left(2q_3\right(k\left)q_4\right(k)+2q_1(k\left)q_2\right(k\left)\right)$

$\mathrm{\&phi;}\left(k\right)=atan2\left(2q_1\right(k\left)q_3\right(k)-2q_2(k\left)q_4\right(k),q_1^2(k)-q_2^2(k)-q_3^2(k)+q_4^2(k\left)\right)$

$\mathrm{\&psi;}\left(k\right)=atan2\left(2q_1\right(k\left)q_4\right(k)-2q_2(k\left)q_3\right(k),q_1^2(k)-q_2^2(k)+q_3^2(k)-q_4^2(k\left)\right)$

方向余弦矩阵更新：

$C_b^n\left(k\right)=\left[\begin{array}{ccc}{q}_1^2\left(k\right)+q_2^2\left(k\right)-q_3^2\left(k\right)-q_4^2\left(k\right)& 2q_2\left(k\right)q_3\left(k\right)-2q_1\left(k\right)q_4\left(k\right)& 2q_2\left(k\right)q_4\left(k\right)+2q_1\left(k\right)q_3\left(k\right)\\ 2q_2\left(k\right)q_3\left(k\right)+2q_1\left(k\right)q_4\left(k\right)& q_1^2\left(k\right)-q_2^2\left(k\right)+q_3^2\left(k\right)-q_4^2\left(k\right)& 2q_3\left(k\right)q_4\left(k\right)+2q_1\left(k\right)q_2\left(k\right)\\ 2q_2\left(k\right)q_4\left(k\right)+2q_1\left(k\right)q_3\left(k\right)& 2q_3\left(k\right)q_4\left(k\right)+2q_1\left(k\right)q_2\left(k\right)& q_1^2\left(k\right)-q_2^2\left(k\right)-q_3^2\left(k\right)+q_4^2\left(k\right)\end{array}\right]$

速度更新：

$\left[\begin{array}{c}{\mathrm{VE}}_{INS}\left(k\right)\\ {\mathrm{VN}}_{INS}\left(k\right)\\ {\mathrm{VU}}_{INS}\left(k\right)\end{array}\right]=\left[\begin{array}{c}{\mathrm{VE}}_{INS}(k-1)\\ {\mathrm{VN}}_{INS}(k-1)\\ {\mathrm{VU}}_{INS}(k-1)\end{array}\right]+0.02\left(C_b^n\&CenterDot;\left[\begin{array}{c}{a}_x\left(k\right)\\ a_y\left(k\right)\\ a_z\left(k\right)\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]\right)$

这里，g表示当地的重力加速度值。

位置更新

$\left[\begin{array}{c}{L}_{INS}\left(k\right)\\ {\mathrm{\&lambda;}}_{INS}\left(k\right)\\ H_{INS}\left(k\right)\end{array}\right]=\left[\begin{array}{c}{L}_{INS}(k-1)\\ {\mathrm{\&lambda;}}_{INS}(k-1)\\ H_{INS}(k-1)\end{array}\right]+0.02\left[\begin{array}{ccc}0& \frac{1}{R_N+h}& 0\\ \frac{1}{(R_E+h){\mathrm{cosL}}_{INS}(k-1)}& 0& 0\\ 0& 0& 1\end{array}\right]\&CenterDot;\left[\begin{array}{c}{\mathrm{VE}}_{INS}\left(k\right)\\ {\mathrm{VN}}_{INS}\left(k\right)\\ {\mathrm{VU}}_{INS}\left(k\right)\end{array}\right]$

这里，R_N和R_E分别表示当地的大地子午圈和卯酉圈曲率半径

步骤14：DSP控制器6发送当前解算的姿态、速度、位置以及采集的传感器数据，包括三轴加速度、三轴角速度、三轴磁场强度和气压高，以及运用公式(1)解算的当前姿态角观测量和γ(k)。发送完成后，惯导计算机修正姿态、速度和位置误差。修正方法是：

η＝η+△

这里，

η＝[L_INS(k) λ_INS(k) H_INS(k) VE_INS(k) VN_INS(k) VU_INS(k) θ φ ψ]^T，

△表示对应的修正量，在有DSP控制器6发送数据的时刻△为接收到的值，其余时刻△为0。

步骤15：DSP控制器6接收数据采集处理器1的数据，并始终只保留最新一次解算的结果。接收GPS定位数据。每一次接收完成后，对GPS做前置滤波。具体方法是：

1)建立GPS前置滤波器的状态方程

其中：

x＝[λ_GPS L_GPS H_GPS VE_GPS VN_GPS VU_GPS]^T

$\mathrm{\&Phi;}\left(k\right)=\left[\begin{array}{cccccc}1& 0& 0& \frac{1}{(R_E+H_{GPS}(k-1\left)\right){\mathrm{cosL}}_{GPS}(k-1)}& 0& 0\\ 0& 1& 0& 0& \frac{1}{R_N+H_{GPS}(k-1)}& 0\\ 0& 0& 1& 0& 0& 1\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

表示当前时刻的过程噪声，其协方差矩阵为：

$Q\left(k\right)=\left[\begin{array}{cc}\frac{S^2{\mathrm{\&sigma;}}_v^2}{3}& \frac{{\mathrm{S\&sigma;}}_v^2}{2}\\ \frac{{\mathrm{S\&sigma;}}_v^2}{2}& {\mathrm{\&sigma;}}_v^2\end{array}\right]$

其中 ${\mathrm{\&sigma;}}_v^2=diag\left\{\left[\begin{array}{ccc}{\mathrm{\&sigma;}}_{{\mathrm{VE}}_{GPS}}^2& {\mathrm{\&sigma;}}_{{\mathrm{VN}}_{GPS}}^2& {\mathrm{\&sigma;}}_{{\mathrm{VU}}_{GPS}}^2\end{array}\right]\right\}$

$S=\left[\begin{array}{ccc}\frac{1}{(R_E+H_{GPS}(k-1\left)\right){\mathrm{cosL}}_{GPS}\left(k\right)}& 0& 0\\ 0& \frac{1}{R_N+H_{GPS}(k-1)}& 0\\ 0& 0& 1\end{array}\right]$

2)建立GPS前置滤波器观测方程

Z(k)＝H·x(k)+v(k)

其中，x(k)为当前时刻接收到的GPS接收机数据，H为6阶单位矩阵，v(k)表示观测噪声，其协方差矩阵为：

$R\left(k\right)=diag\left\{\left[\begin{array}{cccccc}{\mathrm{\&sigma;}}_{{\mathrm{\&lambda;}}_{GPS}}^2& {\mathrm{\&sigma;}}_{L_{GPS}}^2& {\mathrm{\&sigma;}}_{H_{GPS}}^2& {\mathrm{\&sigma;}}_{{\mathrm{VE}}_{GPS}}^2& {\mathrm{\&sigma;}}_{{\mathrm{VN}}_{GPS}}^2& {\mathrm{\&sigma;}}_{{\mathrm{VU}}_{GPS}}^2\end{array}\right]\right\}$

3)对GPS数据做卡尔曼滤波[引用文献2]，得到GPS接收机输出的位置、速度信息的滤波后数据，包括经纬度、东北天三向速度。

步骤16：组合导航计算机利用惯导计算机发送的数据和步骤15处理后的数据，计算惯性导航系统的修正量。具体方法如下：

1)建立组合导航状态方程

这里，

x＝[△L_INS △λ_INS △H_INS △VE_INS △VN_INS △VU_INS △θ △φ

△ψ acc_b_x acc_b_y acc_b_z gyro_b_x gyro_b_y gyro_b_z]^T

$F=\left[\begin{array}{ccccc}{F}_{rr}& F_{rv}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ F_{vr}& F_{vv}& \&lsqb;f^n\&times;\&rsqb;& C_b^n& 0_{3\&times;3}\\ F_{Ar}& F_{Av}& -\&lsqb;w_{in}^n\&times;\&rsqb;& 0_{3\&times;3}& -C_b^n\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right],G=\left[\begin{array}{cc}{0}_{3\&times;3}& 0_{3\&times;3}\\ C_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& -C_b^n\\ I_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& I_{3\&times;3}\end{array}\right],$ 惯性导航系统误差传递模型的过程噪声，其协方差矩阵为：

$Q=diag\&lsqb;{\mathrm{\&sigma;}}_{Acce,x}^2,{\mathrm{\&sigma;}}_{Acce,y}^2,{\mathrm{\&sigma;}}_{Acce,z}^2,{\mathrm{\&sigma;}}_{Gyro,x}^2,{\mathrm{\&sigma;}}_{Gyro,y}^2,{\mathrm{\&sigma;}}_{Gyro,z}^2\&rsqb;$

其中，

$F_{rr}=\left[\begin{array}{ccc}0& 0& \frac{-{\mathrm{VN}}_{INS}(k-1)}{{(R_N+H_{INS}(k-1\left)\right)}^2}\\ \frac{{\mathrm{VE}}_{INS}(k-1){\mathrm{sinL}}_{INS}(k-1)}{(R_E+H_{INS}(k-1\left)\right){\cos }^2{L}_{INS}(k-1)}& 0& \frac{-{\mathrm{VE}}_{INS}(k-1)}{{(R_E+H_{INS}(k-1\left)\right)}^2{\mathrm{cosL}}_{INS}(k-1)}\\ 0& 0& 0\end{array}\right]$

$F_{rv}==\left[\begin{array}{ccc}0& \frac{1}{R_N+H_{INS}(k-1)}& 0\\ \frac{1}{(R_E+H_{INS}(k-1\left)\right){\mathrm{cosL}}_{INS}(k-1)}& 0& 0\\ 0& 0& 1\end{array}\right]$

$\begin{array}{c}{F}_{vv}=\left[\begin{array}{c}\frac{{\mathrm{VN}}_{INS}(k-1){\mathrm{tanL}}_{INS}(k-1)-{\mathrm{VU}}_{INS}(k-1)}{R_E+H_{INS}(k-1)}\\ \frac{-2{\mathrm{VE}}_{INS}(k-1){\mathrm{tanL}}_{INS}(k-1)}{R_E+H_{INS}(k-1)}-2w_e{\mathrm{sinL}}_{INS}(k-1)\\ \frac{2{\mathrm{VE}}_{INS}(k-1)}{R_E+H_{INS}(k-1)}+2w_e{\mathrm{cosL}}_{INS}(k-1)\end{array}\right.\\ \left.\begin{array}{cc}2w_e{\mathrm{sinL}}_{INS}(k-1)+\frac{{\mathrm{VE}}_{INS}(k-1){\mathrm{tanL}}_{INS}(k-1)}{R_E+H_{INS}(k-1)}& -2w_e{\mathrm{cosL}}_{INS}(k-1)-\frac{{\mathrm{VE}}_{INS}(k-1)}{R_E+H_{INS}(k-1)}\\ \frac{-{\mathrm{VU}}_{INS}(k-1)}{R_N+H_{INS}(k-1)}& -\frac{{\mathrm{VN}}_{INS}(k-1)}{R_N+H_{INS}(k-1)}\\ \frac{2{\mathrm{VN}}_{INS}(k-1)}{R_N+H_{INS}(k-1)}& 0\end{array}\right]\end{array}$

$F_{Av}=\left[\begin{array}{ccc}0& \frac{-1}{R_N+H_{INS}(k-1)}& 0\\ \frac{1}{R_E+H_{INS}(k-1)}& 0& 0\\ \frac{{\mathrm{tanL}}_{INS}(k-1)}{(R_E+H_{INS}(k-1\left)\right)}& 0& 0\end{array}\right]$

$F_{Av}=\left[\begin{array}{ccc}0& 0& \frac{{\mathrm{VN}}_{INS}(k-1)}{{(R_N+H_{INS}(k-1\left)\right)}^2}\\ -w_e{\mathrm{sinL}}_{INS}(k-1)& 0& \frac{-{\mathrm{VE}}_{INS}(k-1)}{{(R_E+H_{INS}(k-1\left)\right)}^2}\\ w_e{\mathrm{cosL}}_{INS}(k-1)+\frac{{\mathrm{VN}}_{INS}(k-1)}{(R_E+H_{INS}(k-1\left)\right){\cos }^2{L}_{INS}(k-1)}& 0& \frac{-{\mathrm{VE}}_{INS}{(k-1)}_E{\mathrm{tanL}}_{INS}(k-1)}{{(R_E+H_{INS}(k-1\left)\right)}^2}\end{array}\right]$

[fⁿ×]是由组成的反对称矩阵，w_e表示地球自转角速度，是由 $w_{in}=\left[\begin{array}{c}\frac{-{\mathrm{VN}}_{INS}(k-1)}{R_N+H_{INS}(k-1)}\\ w_e\cos L_{INS}(k-1)+\frac{{\mathrm{VE}}_{INS}(k-1)}{R_E+H_{INS}(k-1)}\\ w_e\sin L_{INS}(k-1)+\frac{{\mathrm{VE}}_{INS}{(k-1)}_E{\mathrm{tanL}}_{INS}(k-1)}{R_E+H_{INS}(k-1)}\end{array}\right]$ 组成的反对称矩阵。

2)建立系统观测方程

Z(k)＝H·x(k)+v(k)

这里，

$H=\left[\begin{array}{ccccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right]$

$R=\left[\begin{array}{ccccc}{\mathrm{\&sigma;}}_{p,GPS}^2& 0& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{V,GPS}^2& 0& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{h,Baro}^2& 0& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{V,Baro}^2& 0\\ 0& 0& 0& 0& {\mathrm{\&sigma;}}_{angle}^2\end{array}\right]$

3)离散化状态方程，并做卡尔曼滤波。

步骤17：将步骤16的组合导航结果发送至惯导计算机并用步骤14的方法修正惯导参数。发送完成后重复执行步骤15-16。

步骤18：惯导计算机判断，接收到的三个角度修正量的变化均小于1度时，点亮LED4，表示初始对准完成，并通知飞控系统可以起飞，同时将当前的姿态角、速度和位置信息发送至飞控系统。飞行系统收到后将当前的最大控制脉宽发送至导航系统，导航系统根据预处理建立的MC1和MC2模型分别计算磁航向角、气压高度和天向速度的方差估计值

步骤19：重复执行步骤17、18和步骤13，即可实现完整的多旋翼飞行器导航应用。

本发明的优点：为多旋翼飞行器提供一种低成本、高精度和稳定连续的姿态、速度和位置输出的导航系统。根据多旋翼飞行器的动态特性及低成本传感器的误差特点，为飞行器提供准确的姿态、速度和位置信息。本发明具有较高的鲁棒性，功耗低，计算量小，适合低成本导航的工程应用。

以上所述仅是对本发明的较佳实施方式而已，并非对本发明作任何形式上的限制，凡是依据本发明的技术实质对以上实施方式所做的任何简单修改，等同变化与修饰，均属于本发明技术方案的范围内。

[引用文献1]D.Gebre-Egziabher,G.H.Elkaim,J.D.Powell,and B.W.Parkinson,“A non-linear,two-step estimation algorithm for calibrating solid-statestrapdown magnetometers,”in Proc.Int.Conf.Integrated Navigation Systems,St.Petersburg,Russia,May 28–30,2001,pp.290–297

[引用文献2]Kalman,Rudolph Emil."A new approach to linear filtering andprediction problems."Journal of Fluids Engineering 82.1(1960):35-45.

Claims (4)

1.多旋翼飞行器GPS和INS低成本组合导航系统，其特征在于：包括数据采集处理器(1)，数据采集处理器(1)通过IIC总线分别连接陀螺仪(2)、加速度计(3)、磁力计(4)、气压计(5)，数据采集处理器(1)控制陀螺仪(2)、加速度计(3)、磁力计(4)、气压计(5)采集数据，并将采集到的传感器数据发送给DSP控制器(6)，GPS模块(7)将定位数据发送给DSP控制器(6)，DSP控制器(6)将传感器数据和定位数据进行导航解算。

2.按照权利要求1所述多旋翼飞行器GPS和INS低成本组合导航系统，其特征在于：所述气压计(5)的采样频率为10Hz，所述陀螺仪(2)、加速度计(3)、磁力计(4)的采样频率均为50Hz。

3.按照权利要求1所述多旋翼飞行器GPS和INS低成本组合导航系统，其特征在于：所述陀螺仪(2)型号为L3G4200D，加速度计(3)型号为ADXL345，磁力计(4)型号为HMC5883L，气压计(5)型号为BMP085，GPS模块(7)型号为Ublox NEO-6M，数据采集处理器(1)型号为STM32F103VET6，DSP控制器(6)型号为TMS320F28335。

4.按照权利要求1所述多旋翼飞行器GPS和INS低成本组合导航系统进行导航的方法，其特征在于按照以下步骤进行：

步骤1：预处理；

a)低成本组合导航系统安装到飞行器，Y轴指向飞行器机身的正前方，X轴指向机身的右方，Z轴指向机身的正上方；

b)离地1米以上固定飞行器，保证飞行器不会起飞；

c)从静止时刻开始，间隔1min增大电调的PWM控制信号高电平持续时间0.02ms，并实时采集并保存陀螺仪、加速度计、磁力计和气压计的数据；

d)分别对磁力计和气压计估计每个PWM控制信号变化的时间间隔内采集的数据的方差，并作图，纵坐标为方差数据，横坐标为对应的PWM高电平持续时间，使用MATLAB的cftool工具箱对每条曲线进行拟合，得到磁力计和气压计每个轴的数据方差与控制信号之间的关系模型MC1和MC2；

组合导航系统加电启动，点亮LED状态灯(1)，数据采集处理器(1)以50Hz频率通过IIC总线读取陀螺仪(2)、加速度计(3)和磁力计(4)的数据，得到飞行器当前的三轴角速率(w_x、w_y、w_z)、三轴加速度(a_x、a_y、a_z)和三轴磁场强度(m_x、m_y、m_z)；

步骤2：重复步骤1并存储100组三轴角速率和三轴加速度，检测判断是否为飞行器静止状态采样的数据；

步骤3：若步骤2中的静止状态检测不通过，重复步骤1～步骤2，直到满足条件1和条件2同时成立条件为止；

步骤4：步骤2的静止状态检测通过后，估计陀螺仪(2)的零偏，零偏估计完成后，点亮LED状态灯(2)；

步骤5：在10s时间内，手动将组合导航系统绕其X、Y、Z轴旋转一次，计算磁力计(4)零偏，计算完成后，点亮LED状态灯(3)；

步骤6：数据采集处理器(1)以10Hz读取气压计(5)采样数据，连续读取10组，得到初始状态初始气压高(Baro_InitHeight)；

步骤7：DSP控制器(6)通过串口USART3接收当前的GPS定位数据：λ_GPS(经度)、L_GPS(纬度)、H_GPS(高度)、VE_GPS(东向速度)、VN_GPS(北向速度)、VU_GPS(天向速度)；

步骤8：重复步骤7接收30次有效GPS数据，将最新的GPS定位数据发送至DSP控制器(6)；

步骤9：DSP控制器(6)修正陀螺仪(2)和磁力计(4)的零偏，将气压高修正到与GPS高同步，得到当前的气压高采样；

步骤10：分别以50HZ采集陀螺仪、加速度计和磁力计数据，以10Hz采集气压高度计数据，每一次采样数据经过步骤9处理；

步骤11：对陀螺仪(2)和加速度计(4)数据做前置低通滤波；

步骤12：计算惯性导航系统的粗对准初始参数；

步骤13：重复步骤9和步骤10直到任务完成为止，更新姿态、速度和位置；

步骤14：DSP控制器(6)发送当前解算的姿态、速度、位置以及采集的传感器数据，包括三轴加速度、三轴角速度、三轴磁场强度和气压高，以及解算的当前姿态角观测量和γ(k)，发送完成后，惯导计算机修正姿态、速度和位置误差；

步骤15：DSP控制器(6)接收数据采集处理器(1)的数据，并始终只保留最新一次解算的结果，接收GPS定位数据，每一次接收完成后，对GPS做前置滤波；

步骤16：组合导航计算机利用惯导计算机发送的数据和步骤15处理后的数据，计算惯性导航系统的修正量；

步骤17：将步骤16的组合导航结果发送至惯导计算机并用步骤14的方法修正惯导参数，发送完成后重复执行步骤15-16；

步骤18：惯导计算机判断，接收到的三个角度修正量的变化均小于1度时，点亮LED(4)，表示初始对准完成，并通知飞控系统可以起飞，同时将当前的姿态角、速度和位置信息发送至飞控系统；飞行系统收到后将当前的最大控制脉宽发送至导航系统，导航系统根据预处理建立的MC1和MC2模型分别计算磁航向角、气压高度的方差估计值；

步骤19：重复执行步骤17和步骤13，即可实现完整的多旋翼飞行器导航应用。