[go: up one dir, main page]

CN109931926B - 一种基于站心坐标系的小型无人机无缝自主式导航方法 - Google Patents

一种基于站心坐标系的小型无人机无缝自主式导航方法 Download PDF

Info

Publication number
CN109931926B
CN109931926B CN201910270662.3A CN201910270662A CN109931926B CN 109931926 B CN109931926 B CN 109931926B CN 201910270662 A CN201910270662 A CN 201910270662A CN 109931926 B CN109931926 B CN 109931926B
Authority
CN
China
Prior art keywords
coordinate system
navigation
altitude
gnss
sensor
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Expired - Fee Related
Application number
CN201910270662.3A
Other languages
English (en)
Other versions
CN109931926A (zh
Inventor
李德辉
王冠林
唐宁
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shandong Zhiyi Aviation Technology Co ltd
Original Assignee
Shandong Zhiyi Aviation Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shandong Zhiyi Aviation Technology Co ltd filed Critical Shandong Zhiyi Aviation Technology Co ltd
Priority to CN201910270662.3A priority Critical patent/CN109931926B/zh
Publication of CN109931926A publication Critical patent/CN109931926A/zh
Application granted granted Critical
Publication of CN109931926B publication Critical patent/CN109931926B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Navigation (AREA)

Abstract

本发明提供一种基于站心坐标系的小型无人机无缝自主式导航方法，本方法以站心坐标系为导航坐标系，利用多传感器冗余辅助基于低成本的MEMS IMU的捷联惯导，利用扩展卡尔曼滤波完成数据融合，实现无人机的位置、速度和姿态解算。本导航方法采用传感器数据健康状态管理系统，对参与导航数据融合的多传感器数据进行自适应切换，并将状态方程维数和量程方程维数进行固化，有效简化了数据融合算法实现，确保无人机在有无GNSS环境下的无缝自主式导航。

Description

一种基于站心坐标系的小型无人机无缝自主式导航方法
技术领域
本发明涉及组合导航方法技术领域，特别是涉及一种基于站心坐标系的小型无人机无缝自主式导航方法。
背景技术
随着无人机应用范围的推广，越来越要求无人机具备室内室外、有无GNSS定位环境下的导航能力。单独的GNSS辅助惯导系统，在GNSS拒止环境下无法使用；视觉导航传感器辅助惯导，由于视觉导航传感器对光照强度、飞行高度和速度、以及地面特征强度有要求，且为了减少计算量和延时，视觉导航传感器常采用视觉里程计(VO)来辅助惯导，长时间导航下定位误差依然会慢慢发散。为了解决无人机室内外各种环境条件下的导航问题，特别是在GNSS拒止环境下的导航问题，需要将多种导航传感器数据进行融合，且需要优化多传感器的数据采用方法，使得导航数据融合算法不因采用传感器增多而变复杂。
本算法主要针对无人机室内外无缝自主导航，包括在GNSS可用环境，以及GNSS拒止环境下，都能实现无人机的导航与定位功能。同时针对廉价的MEMS IMU、磁力计、气压高度计三类传感器进行了冗余涉及，提高了导航系统的可靠性。通过对多传感器数据的健康管理，确保了各种环境变化时进入数据融合的量测量维数固定，为无人机提供了无缝隙的导航与定位。
发明内容
本发明所要解决的技术问题是：为了克服现有技术中的不足，本发明提供一种基于站心坐标系的小型无人机无缝自主式导航方法，本方法融合了、MEMS IMU、磁力计、气压高度计、激光测距传感器、GNSS、视觉导航传感器的数据信息，采用扩展卡尔曼滤波完成数据融合，在站心坐标系下解算出载体的位置、速度和姿态信息。
本发明解决其技术问题所要采用的技术方案是：
本发明所涉及的导航系统包括安装于载体上的一套主MEMS IMU(简称IMU)、主磁力计和主气压高度计，一套备用IMU、备用磁力计和备用气压高度计，以及激光测距传感器，GNSS和视觉导航传感器，其中，两IMU、两磁力计、视觉导航传感器安装坐标与载体坐标系重合，激光测距传感器安装于载体底部，测量方向与载体坐标系的上方向相反。
其中，主IMU和备用IMU均包含三轴正交陀螺仪和三轴正交加速度计，分别用于测量角速度和加速度(比力)，可为同型传感器，也可以在性能上进行高低搭配；主磁力计和备用磁力计均采用三轴正交磁力计，用于地磁测量，可为同型传感器，也可以在性能上进行高低搭配；主气压高度计和备用气压高度计，均是用于测量大气压强，从而换算出气压高度，可为同型传感器，也可以在性能上进行高低搭配；激光测距传感器能够直接测量传感器和反射点之间的单维距离；GNSS用于测量载体的位置和速度信息；视觉导航传感器应采用视觉里程计(VO)或视觉惯性里程计(VIO)，用于测量载体系下两帧有效数据之间的三维位移和载体系速度。
导航系统的传感器采用传感器健康管理系统进行管理和筛选，确保进入导航数据融合解算的数据有效可靠，在有无GNSS定位环境下，实现载体的导航定位。
IMU、磁力计、气压高度计，三类传感器的冗余设计确保了角速度、加速度(比力)、磁强、气压高度测量的可靠性；低成本激光测距传感器精度高，但作用距离有限，强太阳光照射下将失效，适合室内以及黑暗环境下使用，需要与气压高度简单融合后使用；GNSS能够同时定位与测速，但信号容易被干扰和遮挡，且室内环境完全无法使用；视觉导航传感器采用能够输出三维载体系速度以及帧间载体系位移的传感器，如视觉里程计(VO)或者视觉惯性里程计(VIO)，视觉导航用于在GNSS无法定位的环境下替代GNSS的作用。
本发明涉及到的坐标系包括载体坐标系、站心坐标系、伪站心坐标系、东北天地理坐标系、导航坐标系、地磁东北天坐标系、ECEF坐标系、地球球面坐标系，其中，载体坐标系指以装载导航系统的载体的“右前上”坐标系，安装于载体上的具有坐标的传感器(IMU、磁力计、视觉导航传感器)其安装与载体坐标系重合，激光测距传感器安装于载体底部，测量方向与载体坐标系的“上”轴方向反向；站心坐标系为载体导航起始点为原点的东北天地理坐标系，组合导航的位置估计以站心坐标系表示；伪站心坐标系以伪Home点为原点的东北天地理坐标系；东北天地理坐标系简称地理坐标系，指原点为载体质心，GNSS的速度量测以及状态中的速度估计表示在本坐标系中，且认为等同于表示于站心坐标系中；在本方法中导航坐标系采用站心坐标系；地磁东北天坐标系以磁北为北向，东北天地理坐标系天向轴旋转一个地磁偏角即与地磁东北天坐标系重合；ECEF坐标系，也就是地心地固坐标系，即地球坐标系e，原点为地心；地球球面坐标系原点位于旋转椭球中心，用位置点的经度、纬度和高度来表示球面坐标下的位置。
一种基于站心坐标系的小型无人机无缝自主式导航方法，包括以下步骤：
S1：由导航计算机读取安装于无人机上的主IMU、主磁力计、主气压高度计、备用IMU、备用磁力计、备用气压高度计、激光测距传感器、GNSS以及视觉导航传感器的数据信息，并将读取的数据信息提供给传感器数据健康管理系统；
S2：传感器数据健康状态管理系统对步骤S1获得的所有数据信息进行筛选，筛选出有效传感器数据用于导航数据融合解算，并对气压高度和激光测距高度进行预处理，计算出两者融合高度，该融合高度将作为导航数据融合的高度量测；传感器数据健康状态管理系统还将判断出GNSS与视觉导航传感器是否处于数据有效工作状态，当传感器数据健康状态管理系统判断出GNSS定位有效时，进入步骤S3，当传感器数据健康状态管理系统判断出GNSS无定位，但视觉导航传感器数据有效时，进入步骤S4；
S3：当GNSS定位有效时，如果当前导航周期GNSS数据有更新，将GNSS输出的经纬度转换到以无人机Home点为原点的站心坐标系下，再结合GNSS输出的地理坐标系的速度、以及传感器数据健康状态管理系统筛选出的IMU数据、磁力计数据以及高度数据，通过扩展卡尔曼滤波进行数据融合，解算出无人机的位置、速度和姿态；如果当前导航周期GNSS数据没有更新则直接进行捷联纯惯性导航递推；
S4：当GNSS无定位，但视觉导航传感器数据有效时，如果当前导航周期视觉导航传感器数据有更新，则采用视觉导航传感器输出的载体坐标系下的位移和速度、以及传感器数据健康状态管理系统筛选出的IMU数据、磁力计数据以及高度数据，通过扩展卡尔曼滤波进行数据融合，解算出无人机的位置、速度和姿态；如果当前导航周期视觉导航传感器数据没有更新则直接进行捷联纯惯性导航递推。
具体的，在步骤S2中有效传感器的筛选方法具体为：
对于主IMU和备用IMU、主磁力计和备用磁力计、主气压高度计和备用气压高度计筛选时，通过对主备传感器输出数值的大小进行判断、对主备传感器数据的噪声方差进行统计、以及对传感器自带的健康参数值大小进行判断。MEMS传感器越来越智能化，大都带有健康参数输出，健康参数直接能够反应传感器工作状态，同时结合实际应用环境下的传感器性能参数中的量程、噪声方差特性参数，同时根据主备传感器输出数值的差异大小，可有效判断出各传感器数据是否健康有效。当主IMU/主磁力计/主气压高度计处于正常工作时，就采用主IMU/主磁力计/主气压高度计的数据进行后续导航算法的运算，当判定主IMU/主磁力计/主气压高度计为非正常工作状态时，采用备用IMU/备用磁力计/备用气压高度计的数据进行后续导航算法的运算；其中，IMU、磁力计和气压高度计都是分别进行有效性的判断，相互之间不存在依赖关系，如，IMU可以是主IMU有效，也可以是备用IMU有效；磁力计可以是主磁力计有效，也可以是备用磁力计有效；气压高度计可以是主气压高度计有效，也可以是备用气压高度计有效。
激光测距传感器通过噪声方差统计、数值输出范围、传感器自有健康参数判断出激光测距传感器数据的可信度；其中激光测距传感器的健康参数实际上通过对传感器输出的激光测量距离值的噪声方差统计及超量程距离值个数统计共同确定的，将健康参数值划分到0～1范围内，噪声方差越大以及超量程数据越多，则健康参数值越低。
GNSS定位状态判断，可直接根据接收机定位状态参数、DOP值大小、定位卫星数量判断GNSS是否有效定位，DOP值越小，定位越好，卫星数越多效果越好，可对这些参数设置门限值，低于门限值则判断GNSS定位失效；
视觉导航传感器工作状态判断，视觉导航传感器工作状态与光照强度、特征点强度以及飞行高度相关，光照强度可由视觉导航传感器根据图像亮度或灰度值量化输出，特征点强度同样可以由视觉导航传感器量化输出，而飞行高度则根据导航解算结果得到；根据实际的光照强度、特征点强度以及相对地面的飞行高度，并与视觉导航传感器有效工作下的光照强度、特征点强度以及飞行高度范围对比，确定出视觉导航传感器是否处于有效工作状态。
在步骤S2中，气压高度与激光测距高度计算出一个融合高度，作为扩展卡尔曼滤波EKF数据融合的量测信息，对气压高度与激光测距高度进行融合以计算融合高度的过程如下：
(1)确定初始高度HBaro0
在导航初始时刻，将此时气压高度计输出的气压高度，记为初始气压高度HBaro_T0，将此时激光测距传感器输出激光测距高度，记为初始激光测距高度HLaser_T0，计算用于气压高度变化计算的初始高度HBaro0，并忽略导航初始时刻载体水平姿态角导致的激光测距高度的非垂直性，初始高度HBaro0的计算公式如下：
HBaro0＝HBaro_T0-HLaser_T0 (1)
(2)确定激光测距传感器垂直高度HLaser_vertical
由于导航算法中高度指的是垂直高度，而激光测距传感器因为安装的测量方向与载体系Z轴平行，由于无人机姿态角的存在，激光测距传感器并非一直测量垂直高度，当载体水平姿态角不为0时，激光测距传感器测量的是一个倾斜高度，因此需要利用载体的水平姿态角，将激光测距传感器输出的激光测距高度HLaser转换到垂直高度HLaser_vertical，其计算公式如下：
HLaser_vertical＝HLaser*cosθ*cosγ (2)
式中，θ为以弧度为单位的载体俯仰角，γ为以弧度为单位的无人机横滚角；
(3)计算融合高度H
根据气压高度计输出的气压高度HBaro，结合公式(1)和公式(2)的计算值，用于导航数据融合量测高度的融合高度H的计算公式为：
H＝HLaser_vertical*W+(1-W)*(HBaro-HBaro0)(3)
公式(3)中，W为权重系数，取值范围为0～1，激光测距传感器一般自带数值范围为0～1的健康参数输出，当健康参数偏低时，W值置0，健康参数越高，则W值越大，而激光测距传感器的健康参数实际上通过对传感器输出的激光测量距离值的噪声方差统计及超量程距离值个数统计共同确定。
进一步，在步骤S3和S4的导航数据融合中，量测方程中用到了东北天地理坐标系下的地磁矢量数据，由于磁力计的直接测量数据是基于载体坐标系，需要通过载体坐标系下直接测量的磁力计数据转换得到东北天地理坐标系下的地磁矢量数据。
下面详细介绍本发明中磁力计数据的处理过程，东北天地理坐标系下地磁矢量记为[mE mN mU]T，其计算方法如下：
首先，将载体坐标系下磁力计原始输出进行归一化处理，归一化后的地磁矢量记为[mx my mz]T，将归一化后的地磁矢量转换到东北天地理坐标系：
Figure GDA0004043534210000071
其中，[mE1 mN1 mU1]T为通过姿态矩阵直接转换得到的东北天地理坐标系下地磁矢量；
Figure GDA0004043534210000072
为四元数构成的载体坐标系到导航坐标系的姿态矩阵；
接着，利用式(4)中通过姿态矩阵直接转换得到的东北天地理坐标系下地磁矢量[mE1 mN1 mU1]T，重构出地磁东北天坐标系下的地磁矢量[0mN2 mU2]T：
Figure GDA0004043534210000081
最后，将式(5)中地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T，通过地磁偏角的补偿转换到东北天地理坐标系下的地磁矢量[mE mN mU]T：
Figure GDA0004043534210000082
其中，Mag_dec为以弧度为单位的地磁偏角，由经纬度查询得到。
进一步，基于站心坐标系下的，采用EKF融合经过传感器数据健康管理系统筛选出来的传感器数据，数据融合导航算法状态方程为：
Figure GDA0004043534210000083
其中，
Figure GDA0004043534210000084
为系统状态，
Figure GDA0004043534210000085
为系统噪声；
式(7)具体表示如下：
Figure GDA0004043534210000086
其中，系统状态
Figure GDA0004043534210000087
包括：3维站心坐标系下东北天位置
Figure GDA0004043534210000088
3维东北天地理系下速度
Figure GDA0004043534210000089
4维姿态四元数
Figure GDA00040435342100000810
3维陀螺零偏
Figure GDA00040435342100000811
和3维加速度计零偏
Figure GDA00040435342100000812
共16维；系统噪声
Figure GDA00040435342100000813
包括：3维陀螺仪白噪声
Figure GDA00040435342100000814
3维加速度计白噪声
Figure GDA00040435342100000815
3维陀螺零偏随机游走噪声
Figure GDA00040435342100000816
和3维加速度计零偏随机游走噪声
Figure GDA00040435342100000817
共12维；
Figure GDA00040435342100000818
为四元数
Figure GDA00040435342100000819
的乘法矩阵；
Figure GDA00040435342100000820
为四元数构成的载体坐标系到导航坐标系的坐标转换矩阵；
Figure GDA0004043534210000091
为IMU中加速度计输出加速度(比力)、
Figure GDA0004043534210000092
为IMU中陀螺仪输出角速度；根据状态微分方程(8)求解状态一步预测。
导航算法数据融合的量测方程为：
Figure GDA0004043534210000093
其中，
Figure GDA0004043534210000094
表示量测；
Figure GDA0004043534210000095
为量测由状态量表示的非线性函数；
Figure GDA0004043534210000096
表示量测噪声；量测噪声
Figure GDA0004043534210000097
共10维，与10维量测量相对应：3维位置量测噪声、3维速度量测噪声、3维归一化后的磁力计输出量测噪声、1维高度量测噪声；
Figure GDA0004043534210000098
式(10)中，
Figure GDA0004043534210000099
表示站心坐标系下的3维GNSS位置或3维视觉导航传感器位置(即载体系下的视觉有效图像帧间位移)、
Figure GDA00040435342100000910
表示地理坐标系下的3维GNSS速度或3维视觉导航传感器载体系速度、
Figure GDA00040435342100000911
表示3维由磁力计输出归一化后的磁力计量测、H由式(3)得到的1维融合高度。
式(10)中，当GNSS的位置和速度作为量测，
Figure GDA00040435342100000912
可以表示为：
Figure GDA00040435342100000913
式(10)中，当视觉导航传感器的位置和速度作为量测，
Figure GDA00040435342100000914
可以表示为：
Figure GDA00040435342100000915
式(11)和(12)中，
Figure GDA0004043534210000101
为归一化的当地地理坐标系下的地磁分量；PU为状态
Figure GDA0004043534210000102
中的高度；
Figure GDA0004043534210000103
为上一次融合了视觉导航传感器数据的导航解算的位置输出；将状态一步预测值代入式(11)或(12)，即得到量测预测值；
状态
Figure GDA0004043534210000104
一步预测值
Figure GDA0004043534210000105
通过解微分方程获得：
Figure GDA0004043534210000106
式(13)中
Figure GDA0004043534210000107
为上一导航周期状态估计值，T代表导航周期；
状态转移矩阵Φ计算：
Figure GDA0004043534210000108
式(14)中I为单位矩阵；
系统噪声驱动矩阵Γ计算：
Figure GDA0004043534210000109
量测矩阵H计算：
Figure GDA00040435342100001010
式(16)中，GNSS参与导航时对应式(11)用于计算量测矩阵，当视觉导航传感器参与导航时对应式(12)用于计算量测矩阵。
无人机的起飞环境是多种多样的，当起飞时，无人机处于开阔环境中，此时，GNSS定位有效，可以正常输出，输出数据可以直接参与融合运算，并根据Home点的经度、纬度和高度，可将导航数据融合解算出来的站心坐标系下的位置转换为经度、纬度和高度，即使飞行中某些环境下GNSS定位失效，采用了视觉导航传感器的数据参与导航数据融合解算，也同样可根据已知的Home点的经度、纬度和高度，将导航解算的站心坐标系下的位置转换为经度、纬度和高度；当无人机在有遮挡的环境中起飞时，如：室内、桥下、树林中等，此时，无法采用GNSS进行定位，GNSS无输出，只能采用视觉导航传感器进行定位，此时，只能获得无人机的相对位置，而无法获得其绝对位置，即无法确定无人机所在位置的经度、纬度和高度，因此，当GNSS恢复定位时需要反推出Home点的经度、纬度和高度，得到了Home点的经度、纬度和高度后，后续导航解算得到的站心坐标系下的位置就同样可以转换为经度、纬度和高度。
由于导航解算以站心坐标系作为导航坐标系，位置量测和位置估计均以站心坐标系表示，而GNSS输出的位置为经度、纬度和高度，需要将其转换到站心坐标系下后作为量测；站心坐标系下的位置估计在某些应用要求时可能也需要转化为经度、纬度和高度后再输出。主要涉及位置估计和位置量测的坐标转换，下面分别针对起飞时GNSS有效以及起飞时GNSS无效的情况进行详细说明。
(1)当无人机起飞时及后续飞行过程中，GNSS定位一直处于有效环境下，所需要的坐标转换如下：
站心坐标系原点为无人机起始点，即Home点，其坐标为(0 0 0)，在GNSS定位有效环境下，通过GNSS获取Home点经度、纬度和高度，分别记为λ、L、h；导航过程中，GNSS输出的经度、纬度和高度分别记为λS、LS和hS，将导航过程中，GNSS输出的经度、纬度和高度转化为站心坐标系下的位置量测
Figure GDA0004043534210000111
方法如下：
a.将Home点的经度λ、纬度L和高度h转换到ECEF坐标系下的坐标为：(HomeECEFxHomeECEFy HomeECEFz)，其转换公式如下：
Figure GDA0004043534210000121
其中，RN为需要坐标转换的位置点的卯酉圈的曲率半径，e为旋转椭球扁率。
b.导航过程中，GNSS输出的经度λS、纬度LS和高度hS同样转换到ECEF坐标系下的坐标为：(SATECEFx SATECEFy SATECEFz)，其转换方法与步骤a相同，为将λS、LS、hS代入上式(17)，RN为本坐标转换位置点的卯酉圈的曲率半径；
c.根据步骤a和步骤b的结果，GNSS输出的经度λS、纬度LS和高度hS转换为站心坐标系下的位置，并将站心坐标系下的位置作为量测方程中的位置量测，位置量测公式如下：
Figure GDA0004043534210000122
式(18)中
Figure GDA0004043534210000123
由Home点的经度λ和纬度L构成：
Figure GDA0004043534210000124
站心坐标系下的导航位置输出
Figure GDA0004043534210000125
转化为地球球面坐标系下的经度、纬度和高度的方法如下：
a.将站心坐标系下导航位置输出转换为ECEF坐标系下的坐标[X Y Z]T，转换公式为：
Figure GDA0004043534210000126
其中，
Figure GDA0004043534210000127
同式(20)，同样由Home点的经纬度构成；
b.然后根据三角函数关系及迭代计算将ECEF坐标转化成地球球面坐标系下的经度、纬度和高度。
当飞行过程中GNSS定位失效，则用视觉导航传感器替代GNSS功能，解算出的位置
Figure GDA0004043534210000131
同样采用以上步骤可将其转换到地球球面坐标下的经度、纬度和高度。
(2)当起飞时GNSS定位无效，经过一段时间恢复有效，反推GNSS定位无效时Home点的经度、纬度和高度及其ECEF坐标。
当无人机起飞时Home点无GNSS定位，其地球球面坐标下的经度、纬度和高度未知情况下，导航系统依靠视觉传感器的位置与速度信息代替GNSS功能；在飞行过程中GNSS恢复定位时，根据无人机导航系统此时刻解算的站心坐标系下的位置
Figure GDA0004043534210000132
以及GNSS给出的地球球面坐标系下的经度λS、纬度LS和高度hS，反推出Home点的经度λ、纬度L和高度h及其ECEF坐标(HomeECEFx HomeECEFy HomeECEFz)，计算方法如下：
a.在无人机当前所在位置建立伪站心坐标系，当前位置即为伪站心坐标系的原点(即伪Home点)，则真实的站心坐标原点(Home点)在伪站心坐标系下的坐标为(-PE -PN -PU)；
b.GNSS输出的经度λS、纬度LS和高度hS，转换到ECEF坐标系下的坐标(SATECEFxSATECEFy SATECEFz)，即为伪Home点的ECEF坐标；
c.然后套用公式(20)计算真实Home点的ECEF坐标系(HomeECEFx HomeECEFyHomeECEFz)；此时公式(20)中，
Figure GDA0004043534210000133
转换矩阵由伪Home点的经度λS和纬度LS构成，Home点的ECEF坐标由伪Home点的ECEF坐标(SATECEFx SATECEFy SATECEFz)替代；
d.得到Home点的ECEF坐标后即可通过三角函数关系及迭代计算得到真实Home点的球面坐标经度λ、纬度L和高度h。
GNSS恢复定位后，完成Home点球面坐标及ECEF坐标的反推，依然根据公式(18)计算GNSS输出的位置量测。
在全程飞行过程中，无GNSS定位的情况下，仅给飞控输出站心坐标系下的位置；当GNSS定位时，即可输出站心坐标下的位置，也能输出地球球面坐标系下的经度、纬度和高度位置给飞控。
进入数据融合的传感器数据发生变化时，自适应改变EKF中对应的量测噪声协方差矩阵R值；当GNSS与视觉导航传感器之间发生切换时，量测矩阵H相应进行切换。
本发明的有益效果是：本方法以站心坐标系为导航坐标系，利用多传感器冗余辅助基于低成本的MEMS IMU的捷联惯导，利用扩展卡尔曼滤波完成数据融合，实现无人机的位置、速度和姿态解算。本导航方法采用传感器数据健康状态管理系统，对参与导航数据融合的多传感器数据进行自适应切换，并将状态方程维数和量程方程维数进行固化，有效简化了数据融合算法实现，确保无人机在有无GNSS环境下的无缝自主式导航。
附图说明
下面结合附图和实施例对本发明作进一步说明。
图1是本发明无缝自主式导航方法的原理框图。
图2是本发明无缝自主式导航方法的运行流程图。
图3是本发明传感器数据健康状态管理系统数据信息筛选原理框图。
具体实施方式
现在结合附图对本发明作详细的说明。此图为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。
如图1所示，本发明所涉及的导航系统包括一套主IMU、主磁力计和主气压高度计，一套备用IMU、备用磁力计和备用气压高度计，以及激光测距传感器，GNSS和视觉导航传感器，其中，
其中，主IMU和备用IMU均包含三轴正交陀螺仪和三轴正交加速度计，分别用于测量角速度和加速度(比力)，可为同型传感器，也可以在性能上进行高低搭配；主磁力计和备用磁力计均采用三轴正交磁力计，用于地磁测量，可为同型传感器，也可以在性能上进行高低搭配；主气压高度计和备用气压高度计，均是用于测量大气压强，从而换算出气压高度，可为同型传感器，也可以在性能上进行高低搭配；激光测距传感器能够直接测量传感器和反射点之间的单维距离；GNSS用于测量载体的位置和速度信息；视觉导航传感器应采用视觉里程计(VO)或视觉惯性里程计(VIO)，用于测量载体系下两帧有效数据之间的三维位移和载体系速度。
导航系统的传感器采用传感器健康管理系统进行管理和筛选，确保进入导航数据融合解算的数据有效可靠，在有无GNSS定位环境下，实现载体的导航定位。
IMU、磁力计、气压高度计，三类传感器的冗余设计确保了角速度、加速度(比力)、磁强、气压高度测量的可靠性；低成本激光测距传感器精度高，但作用距离有限，强太阳光照射下将失效，适合室内以及黑暗环境下使用，需要与气压高度简单融合后使用；GNSS能够同时定位与测速，但信号容易被干扰和遮挡，且室内环境完全无法使用；视觉导航传感器采用能够输出三维载体系速度以及帧间载体系位移的传感器，如视觉里程计(VO)或者视觉惯性里程计(VIO)，视觉导航用于在GNSS无法定位的环境下替代GNSS的作用。
本发明涉及到的坐标系包括载体坐标系、站心坐标系、伪站心坐标系、东北天地理坐标系、导航坐标系、地磁东北天坐标系、ECEF坐标系、地球球面坐标系，其中，载体坐标系指以装载导航系统的载体的“右前上”坐标系，安装于载体上的具有坐标的传感器(IMU、磁力计、视觉导航传感器)其安装与载体坐标系重合，激光测距传感器安装于载体底部，测量方向与载体坐标系的“上”轴方向反向；站心坐标系为载体导航起始点为原点的东北天地理坐标系，组合导航的位置估计以站心坐标系表示；伪站心坐标系以伪Home点为原点的东北天地理坐标系；东北天地理坐标系简称地理坐标系，指原点为载体质心，GNSS的速度量测以及状态中的速度估计表示在本坐标系中，且认为等同于表示于站心坐标系中；在本方法中导航坐标系采用站心坐标系；地磁东北天坐标系以磁北为北向，东北天地理坐标系天向轴旋转一个地磁偏角即与地磁东北天坐标系重合；ECEF坐标系，也就是地心地固坐标系，即地球坐标系e，原点为地心；地球球面坐标系原点位于旋转椭球中心，用位置点的经度、纬度和高度来表示球面坐标下的位置。
如图2所示，一种基于站心坐标系的小型无人机无缝自主式导航方法，包括以下步骤：
S1：由导航计算机读取安装于无人机上的主IMU、主磁力计、主气压高度计、备用IMU、备用磁力计、备用气压高度计、激光测距传感器、GNSS以及视觉导航传感器的数据信息，并将读取的数据信息提供给传感器数据健康管理系统；
S2：传感器数据健康状态管理系统对步骤S1获得的所有数据信息进行筛选，筛选出有效传感器数据用于导航数据融合解算，并对气压高度和激光测距高度进行预处理，计算出两者融合高度，该融合高度将作为导航数据融合的高度量测；传感器数据健康状态管理系统还将判断出GNSS与视觉导航传感器是否处于数据有效工作状态，当传感器数据健康状态管理系统判断出GNSS定位有效时，进入步骤S3，当传感器数据健康状态管理系统判断出GNSS无定位，但视觉导航传感器数据有效时，进入步骤S4；
S3：当GNSS定位有效时，如果当前导航周期GNSS数据有更新，将GNSS输出的经纬度转换到以无人机Home点为原点的站心坐标系下，再结合GNSS输出的地理坐标系的速度、以及传感器数据健康状态管理系统筛选出的IMU数据、磁力计数据以及高度数据，通过扩展卡尔曼滤波进行数据融合，解算出无人机的位置、速度和姿态；如果当前导航周期GNSS数据没有更新则直接进行捷联纯惯性导航递推；
S4：当GNSS无定位，但视觉导航传感器数据有效时，如果当前导航周期视觉导航传感器数据有更新，则采用视觉导航传感器输出的载体坐标系下的位移和速度、以及传感器数据健康状态管理系统筛选出的IMU数据、磁力计数据以及高度数据，通过扩展卡尔曼滤波进行数据融合，解算出无人机的位置、速度和姿态；如果当前导航周期视觉导航传感器数据没有更新则直接进行捷联纯惯性导航递推。
如图3所示，在步骤S2中有效传感器的筛选方法具体为：
对于主IMU和备用IMU、主磁力计和备用磁力计、主气压高度计和备用气压高度计筛选时，通过对主备传感器输出数值的大小进行判断、对主备传感器数据的噪声方差进行统计、以及对传感器自带的健康参数值大小进行判断。MEMS传感器越来越智能化，大都带有健康参数输出，健康参数直接能够反应传感器工作状态，同时结合实际应用环境下的传感器性能参数中的量程、噪声方差特性参数，同时根据主备传感器输出数值的差异大小，可有效判断出各传感器数据是否健康有效。当主IMU/主磁力计/主气压高度计处于正常工作时，就采用主IMU/主磁力计/主气压高度计的数据进行后续导航算法的运算，当判定主IMU/主磁力计/主气压高度计为非正常工作状态时，采用备用IMU/备用磁力计/备用气压高度计的数据进行后续导航算法的运算；其中，IMU、磁力计和气压高度计都是分别进行有效性的判断，相互之间不存在依赖关系，如，IMU可以是主IMU有效，也可以是备用IMU有效；磁力计可以是主磁力计有效，也可以是备用磁力计有效；气压高度计可以是主气压高度计有效，也可以是备用气压高度计有效。
激光测距传感器通过噪声方差统计、数值输出范围、传感器自有健康参数判断出激光测距传感器数据的可信度；其中激光测距传感器的健康参数实际上通过对传感器输出的激光测量距离值的噪声方差统计及超量程距离值个数统计共同确定的，将健康参数值划分到0～1范围内，噪声方差越大以及超量程数据越多，则健康参数值越低。
GNSS定位状态判断，可直接根据接收机定位状态参数、DOP值大小、定位卫星数量判断GNSS是否有效定位，DOP值越小，定位越好，卫星数越多效果越好，可对这些参数设置门限值，低于门限值则判断GNSS定位失效；
视觉导航传感器工作状态判断，视觉导航传感器工作状态与光照强度、特征点强度以及飞行高度相关，光照强度可由视觉导航传感器根据图像亮度或灰度值量化输出，特征点强度同样可以由视觉导航传感器量化输出，而飞行高度则根据导航解算结果得到；根据实际的光照强度、特征点强度以及相对地面的飞行高度，并与视觉导航传感器有效工作下的光照强度、特征点强度以及飞行高度范围对比，确定出视觉导航传感器是否处于有效工作状态。
在步骤S2中，气压高度与激光测距高度计算出一个融合高度，作为扩展卡尔曼滤波EKF数据融合的量测信息，对气压高度与激光测距高度进行融合以计算融合高度的过程如下：
(1)确定初始高度HBaro0
在导航初始时刻，将此时气压高度计输出的气压高度，记为初始气压高度HBaro_T0，将此时激光测距传感器输出激光测距高度，记为初始激光测距高度HLaser_T0，计算用于气压高度变化计算的初始高度HBaro0，并忽略导航初始时刻载体水平姿态角导致的激光测距高度的非垂直性，初始高度HBaro0的计算公式如下：
HBaro0＝HBaro_T0-HLaser_T0 (1)
(2)确定激光测距传感器垂直高度HLaser_vertical
由于导航算法中高度指的是垂直高度，而激光测距传感器因为安装的测量方向与载体系Z轴平行，由于无人机姿态角的存在，激光测距传感器并非一直测量垂直高度，当载体水平姿态角不为0时，激光测距传感器测量的是一个倾斜高度，因此需要利用载体的水平姿态角，将激光测距传感器输出的激光测距高度HLaser转换到垂直高度HLaser_vertical，其计算公式如下：
HLaser_vertical＝HLaser*cosθ*cosγ (2)
式中，θ为以弧度为单位的载体俯仰角，γ为以弧度为单位的无人机横滚角；
(3)计算融合高度H
根据气压高度计输出的气压高度HBaro，结合公式(1)和公式(2)的计算值，用于导航数据融合量测高度的融合高度H的计算公式为：
H＝HLaser_vertical*W+(1-W)*(HBaro-HBaro0) (3)
公式(3)中，W为权重系数，取值范围为0～1，激光测距传感器一般自带数值范围为0～1的健康参数输出，当健康参数偏低时，W值置0，健康参数越高，则W值越大，而激光测距传感器的健康参数实际上通过对传感器输出的激光测量距离值的噪声方差统计及超量程距离值个数统计共同确定。
进一步，在步骤S3和S4的导航数据融合中，量测方程中用到了东北天地理坐标系下的地磁矢量数据，由于磁力计的直接测量数据是基于载体坐标系，需要通过载体坐标系下直接测量的磁力计数据转换得到东北天地理坐标系下的地磁矢量数据。
下面详细介绍本发明中磁力计数据的处理过程，东北天地理坐标系下地磁矢量记为[mE mN mU]T，其计算方法如下：
首先，将载体坐标系下磁力计原始输出进行归一化处理，归一化后的地磁矢量记为[mx my mz]T，将归一化后的地磁矢量转换到东北天地理坐标系：
Figure GDA0004043534210000201
其中，[mE1 mN1 mU1]T为通过姿态矩阵直接转换得到的东北天地理坐标系下地磁矢量；
Figure GDA0004043534210000202
为四元数构成的载体坐标系到导航坐标系的姿态矩阵；
接着，利用式(4)中通过姿态矩阵直接转换得到的东北天地理坐标系下地磁矢量[mE1 mN1 mU1]T，重构出地磁东北天坐标系下的地磁矢量[0mN2 mU2]T：
Figure GDA0004043534210000203
最后，将式(5)中地磁东北天坐标系下的地磁矢量[0mN2 mU2]T，通过地磁偏角的补偿转换到东北天地理坐标系下的地磁矢量[mE mN mU]T：
Figure GDA0004043534210000204
其中，Mag_dec为以弧度为单位的地磁偏角，由经纬度查询得到。
进一步，基于站心坐标系下的，采用EKF融合经过传感器数据健康管理系统筛选出来的传感器数据，数据融合导航算法状态方程为：
Figure GDA0004043534210000211
其中，
Figure GDA0004043534210000212
为系统状态，
Figure GDA0004043534210000213
为系统噪声；
式(7)具体表示如下：
Figure GDA0004043534210000214
其中，系统状态
Figure GDA0004043534210000215
包括：3维站心坐标系下东北天位置
Figure GDA0004043534210000216
3维东北天地理系下速度
Figure GDA0004043534210000217
4维姿态四元数
Figure GDA0004043534210000218
3维陀螺零偏
Figure GDA0004043534210000219
和3维加速度计零偏
Figure GDA00040435342100002110
共16维；系统噪声
Figure GDA00040435342100002111
包括：3维陀螺仪白噪声
Figure GDA00040435342100002112
3维加速度计白噪声
Figure GDA00040435342100002113
3维陀螺零偏随机游走噪声
Figure GDA00040435342100002114
和3维加速度计零偏随机游走噪声
Figure GDA00040435342100002115
共12维；
Figure GDA00040435342100002116
为四元数
Figure GDA00040435342100002117
的乘法矩阵；
Figure GDA00040435342100002118
为四元数构成的载体坐标系到导航坐标系的坐标转换矩阵；
Figure GDA00040435342100002119
为IMU中加速度计输出加速度(比力)、
Figure GDA00040435342100002120
为IMU中陀螺仪输出角速度；根据状态微分方程(8)求解状态一步预测。
导航算法数据融合的量测方程为：
Figure GDA00040435342100002121
其中，
Figure GDA00040435342100002122
表示量测；
Figure GDA00040435342100002123
为量测由状态量表示的非线性函数；
Figure GDA00040435342100002124
表示量测噪声；量测噪声
Figure GDA00040435342100002125
共10维，与10维量测量相对应：3维位置量测噪声、3维速度量测噪声、3维归一化后的磁力计输出量测噪声、1维高度量测噪声；
Figure GDA0004043534210000221
式(10)中，
Figure GDA0004043534210000222
表示站心坐标系下的3维GNSS位置或3维视觉导航传感器位置(即载体系下的视觉有效图像帧间位移)、
Figure GDA0004043534210000223
表示地理坐标系下的3维GNSS速度或3维视觉导航传感器载体系速度、
Figure GDA0004043534210000224
表示3维由磁力计输出归一化后的磁力计量测、H由式(3)得到的1维融合高度。
式(10)中，当GNSS的位置和速度作为量测，
Figure GDA0004043534210000225
可以表示为：
Figure GDA0004043534210000226
式(10)中，当视觉导航传感器的位置和速度作为量测，
Figure GDA0004043534210000227
可以表示为：
Figure GDA0004043534210000228
式(11)和(12)中，
Figure GDA0004043534210000229
为归一化的当地地理坐标系下的地磁分量；PU为状态
Figure GDA00040435342100002210
中的高度；
Figure GDA00040435342100002211
为上一次融合了视觉导航传感器数据的导航解算的位置输出；将状态一步预测值代入式(11)或(12)，即得到量测预测值；
状态
Figure GDA00040435342100002212
一步预测值
Figure GDA00040435342100002213
通过解微分方程获得：
Figure GDA00040435342100002214
式(13)中
Figure GDA00040435342100002215
为上一导航周期状态估计值，T代表导航周期；
状态转移矩阵Φ计算：
Figure GDA0004043534210000231
式(14)中I为单位矩阵；
系统噪声驱动矩阵Γ计算：
Figure GDA0004043534210000232
量测矩阵H计算：
Figure GDA0004043534210000233
式(16)中，GNSS参与导航时对应式(11)用于计算量测矩阵，当视觉导航传感器参与导航时对应式(12)用于计算量测矩阵。
无人机的起飞环境是多种多样的，当起飞时，无人机处于开阔环境中，此时，GNSS定位有效，可以正常输出，输出数据可以直接参与融合运算，并根据Home点的经度、纬度和高度，可将导航数据融合解算出来的站心坐标系下的位置转换为经度、纬度和高度，即使飞行中某些环境下GNSS定位失效，采用了视觉导航传感器的数据参与导航数据融合解算，也同样可根据已知的Home点的经度、纬度和高度，将导航解算的站心坐标系下的位置转换为经度、纬度和高度；当无人机在有遮挡的环境中起飞时，如：室内、桥下、树林中等，此时，无法采用GNSS进行定位，GNSS无输出，只能采用视觉导航传感器进行定位，此时，只能获得无人机的相对位置，而无法获得其绝对位置，即无法确定无人机所在位置的经度、纬度和高度，因此，当GNSS恢复定位时需要反推出Home点的经度、纬度和高度，得到了Home点的经度、纬度和高度后，后续导航解算得到的站心坐标系下的位置就同样可以转换为经度、纬度和高度。
由于导航解算以站心坐标系作为导航坐标系，位置量测和位置估计均以站心坐标系表示，而GNSS输出的位置为经度、纬度和高度，需要将其转换到站心坐标系下后作为量测；站心坐标系下的位置估计在某些应用要求时可能也需要转化为经度、纬度和高度后再输出。主要涉及位置估计和位置量测的坐标转换，下面分别针对起飞时GNSS有效以及起飞时GNSS无效的情况进行详细说明。
(1)当无人机起飞时及后续飞行过程中，GNSS定位一直处于有效环境下，所需要的坐标转换如下：
站心坐标系原点为无人机起始点，即Home点，其坐标为(0 0 0)，在GNSS定位有效环境下，通过GNSS获取Home点经度、纬度和高度，分别记为λ、L、h；导航过程中，GNSS输出的经度、纬度和高度分别记为λS、LS和hS，将导航过程中，GNSS输出的经度、纬度和高度转化为站心坐标系下的位置量测
Figure GDA0004043534210000241
方法如下：
a.将Home点的经度λ、纬度L和高度h转换到ECEF坐标系下的坐标为：(HomeECEFxHomeECEFy HomeECEFz)，其转换公式如下：
Figure GDA0004043534210000242
其中，RN为需要坐标转换的位置点的卯酉圈的曲率半径，e为旋转椭球扁率。
b.导航过程中，GNSS输出的经度λS、纬度LS和高度hS同样转换到ECEF坐标系下的坐标为：(SATECEFx SATECEFy SATECEFz)，其转换方法与步骤a相同，为将λS、LS、hS代入上式(17)，RN为本坐标转换位置点的卯酉圈的曲率半径；
c.根据步骤a和步骤b的结果，GNSS输出的经度λS、纬度LS和高度hS转换为站心坐标系下的位置，并将站心坐标系下的位置作为量测方程中的位置量测，位置量测公式如下：
Figure GDA0004043534210000251
式(18)中
Figure GDA0004043534210000252
由Home点的经度λ和纬度L构成：
Figure GDA0004043534210000253
站心坐标系下的导航位置输出
Figure GDA0004043534210000254
转化为地球球面坐标系下的经度、纬度和高度的方法如下：
a.将站心坐标系下导航位置输出转换为ECEF坐标系下的坐标[X Y Z]T，转换公式为：
Figure GDA0004043534210000255
其中，
Figure GDA0004043534210000256
同式(20)，同样由Home点的经纬度构成；
b.然后根据三角函数关系及迭代计算将ECEF坐标转化成地球球面坐标系下的经度、纬度和高度。
当飞行过程中GNSS定位失效，则用视觉导航传感器替代GNSS功能，解算出的位置
Figure GDA0004043534210000257
同样采用以上步骤可将其转换到地球球面坐标下的经度、纬度和高度。
(2)当起飞时GNSS定位无效，经过一段时间恢复有效，反推GNSS定位无效时Home点的经度、纬度和高度及其ECEF坐标。
当无人机起飞时Home点无GNSS定位，其地球球面坐标下的经度、纬度和高度未知情况下，导航系统依靠视觉传感器的位置与速度信息代替GNSS功能；在飞行过程中GNSS恢复定位时，根据无人机导航系统此时刻解算的站心坐标系下的位置
Figure GDA0004043534210000261
以及GNSS给出的地球球面坐标系下的经度λS、纬度LS和高度hS，反推出Home点的经度λ、纬度L和高度h及其ECEF坐标(HomeECEFx HomeECEFy HomeECEFz)，计算方法如下：
a.在无人机当前所在位置建立伪站心坐标系，当前位置即为伪站心坐标系的原点(即伪Home点)，则真实的站心坐标原点(Home点)在伪站心坐标系下的坐标为(-PE-PN-PU)；
b.GNSS输出的经度λS、纬度LS和高度hS，转换到ECEF坐标系下的坐标(SATECEFxSATECEFy SATECEFz)，即为伪Home点的ECEF坐标；
c.然后套用公式(20)计算真实Home点的ECEF坐标系(HomeECEFx HomeECEFyHomeECEFz)；此时公式(20)中，Cen转换矩阵由伪Home点的经度λS和纬度LS构成，Home点的ECEF坐标由伪Home点的ECEF坐标(SATECEFx SATECEFy SATECEFz)替代；
d.得到Home点的ECEF坐标后即可通过三角函数关系及迭代计算得到真实Home点的球面坐标经度λ、纬度L和高度h。
GNSS恢复定位后，完成Home点球面坐标及ECEF坐标的反推，依然根据公式(18)计算GNSS输出的位置量测。
在全程飞行过程中，无GNSS定位的情况下，仅给飞控输出站心坐标系下的位置；当GNSS定位时，即可输出站心坐标下的位置，也能输出地球球面坐标系下的经度、纬度和高度位置给飞控。
进入数据融合的传感器数据发生变化时，自适应改变EKF中对应的量测噪声协方差矩阵R值；当GNSS与视觉导航传感器之间发生切换时，量测矩阵H相应进行切换。
以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关的工作人员完全可以在不偏离本发明的范围内，进行多样的变更以及修改。本项发明的技术范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。

Claims (4)

1.一种基于站心坐标系的小型无人机无缝自主式导航方法，其特征在于：所述导航方法应用于导航系统，所述导航系统包括一套主IMU、主磁力计和主气压高度计，一套备用IMU、备用磁力计和备用气压高度计，以及激光测距传感器，GNSS和视觉导航传感器；
还包括以下步骤：
S1：由导航计算机读取安装于无人机上的主IMU、主磁力计、主气压高度计、备用IMU、备用磁力计、备用气压高度计、激光测距传感器、GNSS以及视觉导航传感器的数据信息，并将读取的数据信息提供给传感器数据健康管理系统；
S2：传感器数据健康状态管理系统对步骤S1获得的所有数据信息进行筛选，筛选出有效传感器数据用于导航数据融合解算，并对气压高度和激光测距高度进行预处理，计算出两者融合高度，该融合高度将作为导航数据融合的高度量测；传感器数据健康状态管理系统还将判断出GNSS与视觉导航传感器是否处于数据有效工作状态，当传感器数据健康状态管理系统判断出GNSS定位有效时，进入步骤S3，当传感器数据健康状态管理系统判断出GNSS无定位，但视觉导航传感器数据有效时，进入步骤S4；
其中，步骤S2中有效传感器的筛选方法具体为：
对于主IMU和备用IMU、主磁力计和备用磁力计、主气压高度计和备用气压高度计筛选时，主备数据通过数值差异对比、噪声方差统计、数值输出范围、传感器自有健康参数，判断出各传感器数据是否有效，当主IMU/主磁力计/主气压高度计处于正常工作时，就采用主IMU/主磁力计/主气压高度计的数据进行后续导航算法的运算，当判定主IMU/主磁力计/主气压高度计为非正常工作状态时，采用备用IMU/备用磁力计/备用气压高度计的数据进行后续导航算法的运算；
激光测距传感器通过噪声方差统计、数值输出范围、传感器自有健康参数判断出激光测距传感器数据的可信度；
GNSS定位判断，根据接收机定位状态参数、DOP值、定位卫星数量判断GNSS是否有效定位；
视觉导航传感器工作状态判断，视觉导航传感器有光照强度和特征点强度的信息输出，根据光照强度、特征点强度和相对地面的飞行高度，与视觉导航传感器有效工作下的光照强度、特征点强度以及飞行高度范围对比，确定出视觉导航传感器是否处于有效工作状态；
步骤S2中，气压高度与激光测距高度计算出一个融合高度，作为EKF数据融合的量测信息，对气压高度与激光测距高度进行融合以计算融合高度的过程如下：
(1)确定初始高度HBaro0
在导航初始时刻，将此时气压高度计输出的气压高度，记为初始气压高度HBaro_T0，将此时激光测距传感器输出激光测距高度，记为初始激光测距高度HLaser_T0，计算用于气压高度变化计算的初始高度HBaro0，并忽略导航初始时刻载体水平姿态角导致的激光测距高度的非垂直性，初始高度HBaro0的计算公式如下：
HBaro0＝HBaro_T0-HLaser_T0 (1)
(2)确定激光测距传感器垂直高度HLaser_vertical
将激光测距传感器输出的激光测距高度HLaser转换到垂直高度HLaser_vertical，其计算公式如下：
HLaser_vertical＝HLaser*cosθ*cosγ (2)
式中，θ为以弧度为单位的载体俯仰角，γ为以弧度为单位的无人机横滚角；
(3)计算融合高度H
根据气压高度计输出的气压高度HBaro，结合公式(1)和公式(2)的计算值，用于导航数据融合量测高度的融合高度H的计算公式为：
H＝HLaser_vertical*W+(1-W)*(HBaro-HBaro0)(3)
公式(3)中，W为权重系数，取值范围为0～1，激光测距传感器自带数值范围为0～1的健康参数输出，当健康参数偏低时，W值置0，健康参数越高，则W值越大，而激光测距传感器的健康参数实际上通过对传感器输出的激光测量距离值的噪声方差统计及超量程距离值个数统计共同确定；
S3：当GNSS定位有效时，如果当前导航周期GNSS数据有更新，将GNSS输出的经纬度转换到以无人机Home点为原点的站心坐标系下，再结合GNSS输出的地理坐标系的速度、以及传感器数据健康状态管理系统筛选出的IMU数据、磁力计数据以及高度数据，通过扩展卡尔曼滤波进行数据融合，解算出无人机的位置、速度和姿态；如果当前导航周期GNSS数据没有更新则直接进行捷联纯惯性导航递推；
S4：当GNSS无定位，但视觉导航传感器数据有效时，如果当前导航周期视觉导航传感器数据有更新，则采用视觉导航传感器输出的载体坐标系下的位移和速度、以及传感器数据健康状态管理系统筛选出的IMU数据、磁力计数据以及高度数据，通过扩展卡尔曼滤波进行数据融合，解算出无人机的位置、速度和姿态；如果当前导航周期视觉导航传感器数据没有更新则直接进行捷联纯惯性导航递推。
2.如权利要求1所述的一种基于站心坐标系的小型无人机无缝自主式导航方法，其特征在于：在步骤S3和S4的导航数据融合中都存在磁力计数据的处理过程，即东北天地理坐标系下地磁矢量[mE mN mU]T的计算，其计算方法如下：
将载体坐标系下磁力计原始输出的归一化值[mx my mz]T转换到东北天地理坐标系：
Figure FDA0004036193640000041
其中，[mE1 mN1 mU1]T为通过姿态矩阵直接转换得到的东北天地理坐标系下地磁矢量；
Figure FDA0004036193640000042
为四元数构成的载体坐标系到导航坐标系的姿态矩阵；
利用式(4)中东北天地理坐标系下地磁矢量[mE1 mN1 mU1]T，重构出地磁东北天坐标系下的地磁矢量[0mN2 mU2]T：
Figure FDA0004036193640000043
将式(5)中地磁东北天坐标系下的地磁矢量[0mN2 mU2]T，通过地磁偏角的补偿转换到东北天地理坐标系下的地磁矢量[mE mN mU]T：
Figure FDA0004036193640000044
其中，Mag_dec为以弧度为单位的地磁偏角，由经纬度查询得到。
3.如权利要求2所述的一种基于站心坐标系的小型无人机无缝自主式导航方法，其特征在于：基于站心坐标系下的，采用EKF融合经过传感器数据健康管理系统筛选出来的传感器数据，数据融合导航算法状态方程为：
Figure FDA0004036193640000045
其中，
Figure FDA0004036193640000046
为系统状态，
Figure FDA0004036193640000047
为系统噪声；
式(7)具体表示如下：
Figure FDA0004036193640000051
其中，系统状态
Figure FDA0004036193640000052
包括：3维站心坐标系下东北天位置
Figure FDA0004036193640000053
3维东北天地理系下速度
Figure FDA0004036193640000054
4维姿态四元数
Figure FDA0004036193640000055
3维陀螺零偏
Figure FDA0004036193640000056
和3维加速度计零偏
Figure FDA0004036193640000057
共16维；系统噪声
Figure FDA0004036193640000058
包括：3维陀螺仪白噪声
Figure FDA0004036193640000059
3维加速度计白噪声
Figure FDA00040361936400000510
3维陀螺零偏随机游走噪声
Figure FDA00040361936400000511
和3维加速度计零偏随机游走噪声
Figure FDA00040361936400000512
共12维；
Figure FDA00040361936400000513
为四元数
Figure FDA00040361936400000514
的乘法矩阵；
Figure FDA00040361936400000515
为四元数构成的载体坐标系到导航坐标系的坐标转换矩阵；
Figure FDA00040361936400000516
为IMU中加速度计输出加速度，即比力；
Figure FDA00040361936400000517
为IMU中陀螺仪输出角速度；根据状态微分方程(8)求解状态一步预测；
导航算法数据融合的量测方程为：
Figure FDA00040361936400000518
其中，
Figure FDA00040361936400000519
表示量测；
Figure FDA00040361936400000520
为量测由状态量表示的非线性函数；
Figure FDA00040361936400000521
表示量测噪声；量测噪声
Figure FDA00040361936400000522
共10维，与10维量测量相对应：3维位置量测噪声、3维速度量测噪声、3维归一化后的磁力计输出量测噪声、1维高度量测噪声；
Figure FDA00040361936400000523
式(10)中，
Figure FDA00040361936400000524
表示站心坐标系下的3维GNSS位置或3维视觉导航传感器位置、
Figure FDA00040361936400000525
表示地理坐标系下的3维GNSS速度或3维视觉导航传感器载体系速度、
Figure FDA0004036193640000061
表示3维由磁力计输出归一化后的磁力计量测、H由式(3)得到的1维融合高度；
式(10)中，当GNSS的位置和速度作为量测，
Figure FDA0004036193640000062
表示为：
Figure FDA0004036193640000063
式(10)中，当视觉导航传感器的位置和速度作为量测，
Figure FDA0004036193640000064
表示为：
Figure FDA0004036193640000065
式(11)和(12)中，
Figure FDA0004036193640000066
为归一化的当地地理坐标系下的地磁分量；PU为状态
Figure FDA0004036193640000067
中的高度；
Figure FDA0004036193640000068
为上一次融合了视觉导航传感器数据的导航解算的位置输出；将状态一步预测值代入式(11)或(12)，即得到量测预测值；
状态
Figure FDA0004036193640000069
一步预测值
Figure FDA00040361936400000610
通过解微分方程获得：
Figure FDA00040361936400000611
式(13)中
Figure FDA00040361936400000612
为上一导航周期状态估计值，T代表导航周期；
状态转移矩阵Φ计算：
Figure FDA00040361936400000613
式(14)中I为单位矩阵；
系统噪声驱动矩阵Γ计算：
Figure FDA00040361936400000614
量测矩阵H计算：
Figure FDA0004036193640000071
式(16)中，GNSS参与导航时对应式(11)用于计算量测矩阵，当视觉导航传感器参与导航时对应式(12)用于计算量测矩阵。
4.如权利要求3所述的一种基于站心坐标系的小型无人机无缝自主式导航方法，其特征在于：步骤S3和S4中，涉及相关传感器输出数据需要进行坐标的转换，位置量测和位置估计的坐标转换：
(1)当无人机起飞时及后续飞行过程中，GNSS定位一直处于有效环境下，所需要的坐标转换如下：
站心坐标系原点为无人机起始点，即Home点，其坐标为(0 0 0)，在GNSS定位有效环境下，通过GNSS获取Home点经度、纬度和高度，分别记为λ、L、h；导航过程中，GNSS输出的经度、纬度和高度分别记为λS、LS和hS，将导航过程中，GNSS输出的经度、纬度和高度转化为站心坐标系下的位置量测
Figure FDA0004036193640000072
方法如下：
a.将Home点的经度λ、纬度L和高度h转换到ECEF坐标系下，其坐标记为：(HomeECEFxHomeECEFy HomeECEFz)；
b.导航过程中，GNSS输出的经度λS、纬度LS和高度hS同样转换到ECEF坐标系下，其坐标记为：(SATECEFx SATECEFy SATECEFz)；
c.根据步骤a和步骤b的结果，GNSS输出的经度λS、纬度LS和高度hS转换为站心坐标系下的位置，并将站心坐标系下的位置作为量测方程中的位置量测，位置量测公式如下：
Figure FDA0004036193640000081
式(18)中
Figure FDA0004036193640000082
由Home点的经度λ和纬度L构成：
Figure FDA0004036193640000083
然后，将站心坐标系下的导航位置输出
Figure FDA0004036193640000084
转化为地球球面坐标系下的经度、纬度和高度，其转化方法如下：
a.将站心坐标系下导航位置输出转换为ECEF坐标系下的坐标[X Y Z]T，转换公式为：
Figure FDA0004036193640000085
其中，
Figure FDA0004036193640000086
同式(20)，同样由Home点的经纬度构成；
b.然后根据三角函数关系及迭代计算将ECEF坐标转化成地球球面坐标系下的经度、纬度和高度；
(2)当起飞时GNSS定位无效，经过一段时间恢复有效，反推GNSS定位无效时Home点的经度、纬度和高度及其ECEF坐标的过程如下：
当无人机起飞时Home点无GNSS定位，其地球球面坐标系下的经度、纬度和高度未知情况下，导航系统依靠视觉导航传感器的位置与速度信息代替GNSS功能；在飞行过程中GNSS恢复定位时，根据无人机导航系统此时刻解算的站心坐标系下的位置
Figure FDA0004036193640000087
以及GNSS给出的当前位置的地球球面坐标系下的经度λS、纬度LS和高度hS，反推出Home点的经度λ、纬度L和高度h及其ECEF坐标(HomeECEFx HomeECEFy HomeECEFz)，计算方法如下：
a.在无人机当前所在位置建立伪站心坐标系，当前位置即为伪站心坐标系的原点，即伪Home点，则真实的站心坐标原点在伪站心坐标系下的坐标为(-PE -PN -PU)；
b.GNSS输出的经度λS、纬度LS和高度hS，转换到ECEF坐标系下的坐标(SATECEFxSATECEFy SATECEFz)，即为伪Home点的ECEF坐标；
c.然后套用公式(20)计算真实Home点的ECEF坐标系下坐标(HomeECEFx HomeECEFyHomeECEFz)；此时公式(20)中，
Figure FDA0004036193640000091
转换矩阵由伪Home点的经度λS和纬度LS构成，Home点的ECEF坐标由伪Home点的ECEF坐标(SATECEFx SATECEFy SATECEFz)替代；
d.得到Home点的ECEF坐标后即可通过三角函数关系及迭代计算得到真实Home点的球面坐标经度λ、纬度L和高度h；
GNSS恢复定位后，完成Home点球面坐标及ECEF坐标的反推，依然根据公式(18)计算GNSS输出的位置量测。
CN201910270662.3A 2019-04-04 2019-04-04 一种基于站心坐标系的小型无人机无缝自主式导航方法 Expired - Fee Related CN109931926B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910270662.3A CN109931926B (zh) 2019-04-04 2019-04-04 一种基于站心坐标系的小型无人机无缝自主式导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910270662.3A CN109931926B (zh) 2019-04-04 2019-04-04 一种基于站心坐标系的小型无人机无缝自主式导航方法

Publications (2)

Publication Number Publication Date
CN109931926A CN109931926A (zh) 2019-06-25
CN109931926B true CN109931926B (zh) 2023-04-25

Family

ID=66989303

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910270662.3A Expired - Fee Related CN109931926B (zh) 2019-04-04 2019-04-04 一种基于站心坐标系的小型无人机无缝自主式导航方法

Country Status (1)

Country Link
CN (1) CN109931926B (zh)

Families Citing this family (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110941285A (zh) * 2019-11-29 2020-03-31 云南大学 一种基于双ip核的无人机飞行控制系统
CN111158363A (zh) * 2019-12-27 2020-05-15 苏州博田自动化技术有限公司 基于控制时序组合的宏微导航方法
CN111207734B (zh) * 2020-01-16 2022-01-07 西安因诺航空科技有限公司 一种基于ekf的无人机组合导航方法
CN111445491B (zh) * 2020-03-24 2023-09-15 山东智翼航空科技有限公司 微型无人机三邻域极大差值边缘检测狭道导引方法
CN111486841B (zh) * 2020-05-19 2022-04-05 西安因诺航空科技有限公司 一种基于激光定位系统的无人机导航定位方法
CN112558087B (zh) * 2020-11-20 2023-06-23 东风汽车集团有限公司 一种自动驾驶车辆的定位系统及方法
CN112634354B (zh) * 2020-12-21 2021-08-13 紫清智行科技(北京)有限公司 基于路侧传感器的网联自动驾驶行车风险评估方法与装置
CN113177309B (zh) * 2021-04-23 2023-04-07 合肥赛为智能有限公司 一种基于决策算法的无人机冗余传感器的数据分析方法
CN115307626A (zh) * 2021-05-06 2022-11-08 北京航通云科技有限公司 一种应用于小型无人机的冗余定位方法
CN113325454B (zh) * 2021-05-18 2022-06-14 武汉大学 一种基于ArduRover无人车的组合定位方法
CN113237478B (zh) * 2021-05-27 2022-10-14 哈尔滨工业大学 一种无人机的姿态和位置估计方法及无人机
CN114115010B (zh) * 2021-11-18 2024-06-21 中国特种飞行器研究所 一种用于无人热气球的飞行高度控制系统
CN113821059B (zh) * 2021-11-24 2022-02-18 中航金城无人系统有限公司 一种多旋翼无人机传感器故障安全飞行控制系统及方法
CN114625155B (zh) * 2022-05-13 2022-08-19 向量传感科技(宁波)有限公司 自动驾驶软件三重冗余管理系统及方法
CN116448505B (zh) * 2023-06-16 2023-09-01 生态环境部华南环境科学研究所(生态环境部生态环境应急研究所) 基于无人船重力测量的水质检测自动采样方法及系统
CN117148406B (zh) * 2023-10-30 2024-01-30 山东大学 一种室内外无缝弹性融合定位方法、系统、介质及设备
CN118519180B (zh) * 2024-06-13 2024-10-29 比亚迪股份有限公司 导航定位方法、装置、介质和车辆

Family Cites Families (26)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8275193B2 (en) * 2004-08-04 2012-09-25 America Gnc Corporation Miniaturized GPS/MEMS IMU integrated board
CN102506857B (zh) * 2011-11-28 2014-01-22 北京航空航天大学 一种基于双imu/dgps组合的相对姿态测量实时动态滤波方法
US9031782B1 (en) * 2012-01-23 2015-05-12 The United States Of America As Represented By The Secretary Of The Navy System to use digital cameras and other sensors in navigation
CN103344218A (zh) * 2013-06-18 2013-10-09 桂林理工大学 一种适用于低空无人机的高度测量系统及方法
EP3470787B1 (en) * 2013-11-27 2020-04-15 The Trustees Of The University Of Pennsylvania Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (mav)
CN103697889B (zh) * 2013-12-29 2016-05-25 北京航空航天大学 一种基于多模型分布式滤波的无人机自主导航与定位方法
US20150219767A1 (en) * 2014-02-03 2015-08-06 Board Of Regents, The University Of Texas System System and method for using global navigation satellite system (gnss) navigation and visual navigation to recover absolute position and attitude without any prior association of visual features with known coordinates
CN104697520B (zh) * 2015-02-05 2017-10-31 南京航空航天大学 一体化无陀螺捷联惯导系统与gps系统组合导航方法
CN104729506B (zh) * 2015-03-27 2017-11-14 北京航空航天大学 一种视觉信息辅助的无人机自主导航定位方法
CN105571591A (zh) * 2015-12-15 2016-05-11 中国电子科技集团公司第二十六研究所 一种多信息深组合导航微系统及导航方法
CN105628024B (zh) * 2015-12-29 2018-09-04 中国电子科技集团公司第二十六研究所 基于多传感器融合的单人定位导航器和定位导航方法
US10295365B2 (en) * 2016-07-29 2019-05-21 Carnegie Mellon University State estimation for aerial vehicles using multi-sensor fusion
US10151588B1 (en) * 2016-09-28 2018-12-11 Near Earth Autonomy, Inc. Determining position and orientation for aerial vehicle in GNSS-denied situations
CN106595653A (zh) * 2016-12-08 2017-04-26 南京航空航天大学 一种穿戴式行人自主导航系统及其导航方法
CN106780699B (zh) * 2017-01-09 2020-10-16 东南大学 一种基于sins/gps和里程计辅助的视觉slam方法
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
CN107024206A (zh) * 2017-04-17 2017-08-08 重庆邮电大学 一种基于ggi/gps/ins的组合导航系统
CN107747940A (zh) * 2017-05-11 2018-03-02 南京继航科技有限公司 一种基于fpga及rtos的多传感器融合导航装置
CN107289948A (zh) * 2017-07-24 2017-10-24 成都通甲优博科技有限责任公司 一种基于多传感器融合的无人机导航系统及方法
CN109387192B (zh) * 2017-08-02 2022-08-26 湖南云箭格纳微信息科技有限公司 一种室内外连续定位方法及装置
CN107478221A (zh) * 2017-08-11 2017-12-15 黄润芳 一种用于移动终端的高精度定位方法
CN107907900A (zh) * 2017-11-07 2018-04-13 长光卫星技术有限公司 一种gnss双天线辅助的多传感器组合导航系统及方法
CN108957511A (zh) * 2018-05-21 2018-12-07 永康威力科技股份有限公司 一种车辆导航转向控制系统和导航修正方法
CN109000640B (zh) * 2018-05-25 2021-09-28 东南大学 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN109059904A (zh) * 2018-06-01 2018-12-21 浙江亚特电器有限公司 用于移动载具的组合导航方法
CN108375370B (zh) * 2018-07-02 2019-03-01 江苏中科院智能科学技术应用研究院 一种面向智能巡防无人机的复合导航系统

Also Published As

Publication number Publication date
CN109931926A (zh) 2019-06-25

Similar Documents

Publication Publication Date Title
CN109931926B (zh) 一种基于站心坐标系的小型无人机无缝自主式导航方法
CN110274588B (zh) 基于无人机集群信息的双层嵌套因子图多源融合导航方法
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN108375370B (zh) 一种面向智能巡防无人机的复合导航系统
CN110095800A (zh) 一种多源融合的自适应容错联邦滤波组合导航方法
CN105865454B (zh) 一种基于实时在线地图生成的无人机导航方法
CN106767752B (zh) 一种基于偏振信息的组合导航方法
CN111412912A (zh) 导航板卡、用于导航板卡的多源数据融合方法以及运载体
CN114646992B (zh) 定位方法、装置、计算机设备、存储介质和计算机程序产品
CN112505737A (zh) 一种基于Elman神经网络在线学习辅助的GNSS/INS组合导航方法
CN113834483B (zh) 一种基于可观测度的惯性/偏振/地磁容错导航方法
CN106949889A (zh) 针对行人导航的低成本mems/gps组合导航系统及方法
CN114111776B (zh) 定位方法及相关装置
CN111025366A (zh) 基于ins及gnss的网格slam的导航系统及方法
CN114018242A (zh) 一种基于偏振/太阳/惯性信息智能匹配的自主定姿方法
CN118089710A (zh) 一种基于imu和lstm辅助的惯性导航性能保持方法
Li et al. Unmanned aerial vehicle position estimation augmentation using optical flow sensor
CN109490931A (zh) 飞行定位方法、装置及无人机
Song et al. R²-gvio: A robust, real-time gnss-visual-inertial state estimator in urban challenging environments
CN108613675A (zh) 低成本无人飞行器移动测量方法及系统
CN112129288B (zh) 一种基于偏振光/地磁航向约束的位姿估计方法及系统
CN110779514A (zh) 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置
CN116753951A (zh) 基于多传感器融合的行人定位方法及系统
CN113819904B (zh) 一种基于天顶点矢量的偏振/vio三维姿态确定方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20230425