CN109931926A - 一种基于站心坐标系的小型无人机无缝自主式导航算法 - Google Patents
一种基于站心坐标系的小型无人机无缝自主式导航算法 Download PDFInfo
- Publication number
- CN109931926A CN109931926A CN201910270662.3A CN201910270662A CN109931926A CN 109931926 A CN109931926 A CN 109931926A CN 201910270662 A CN201910270662 A CN 201910270662A CN 109931926 A CN109931926 A CN 109931926A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- navigation
- height
- sensor
- gnss
- 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.)
- Granted
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 39
- 230000004927 fusion Effects 0.000 claims abstract description 63
- 230000036541 health Effects 0.000 claims abstract description 54
- 238000005259 measurement Methods 0.000 claims description 93
- 230000000007 visual effect Effects 0.000 claims description 84
- 238000000034 method Methods 0.000 claims description 42
- 239000011159 matrix material Substances 0.000 claims description 39
- 238000004364 calculation method Methods 0.000 claims description 38
- 238000006243 chemical reaction Methods 0.000 claims description 30
- 230000008569 process Effects 0.000 claims description 21
- 239000004576 sand Substances 0.000 claims description 18
- 238000005286 illumination Methods 0.000 claims description 14
- 238000006073 displacement reaction Methods 0.000 claims description 9
- 238000001914 filtration Methods 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 7
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 6
- 238000005295 random walk Methods 0.000 claims description 6
- 230000002159 abnormal effect Effects 0.000 claims description 3
- 150000001875 compounds Chemical class 0.000 claims description 3
- 238000012886 linear function Methods 0.000 claims description 3
- 238000012216 screening Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims 1
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 4
- 230000009466 transformation Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000009434 installation Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
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
由于导航算法中高度指的是垂直高度，而激光测距传感器因为安装的测量方向与载体系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，将归一化后的地磁矢量转换到东北天地理坐标系：
其中，[mE1 mN1 mU1]T为通过姿态矩阵直接转换得到的东北天地理坐标系下地磁矢量；为四元数构成的载体坐标系到导航坐标系的姿态矩阵；
接着，利用式(4)中通过姿态矩阵直接转换得到的东北天地理坐标系下地磁矢量[mE1 mN1 mU1]T，重构出地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T：
最后，将式(5)中地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T，通过地磁偏角的补偿转换到东北天地理坐标系下的地磁矢量[mE mN mU]T：
其中，Mag_dec为以弧度为单位的地磁偏角，由经纬度查询得到。
进一步，基于站心坐标系下的，采用EKF融合经过传感器数据健康管理系统筛选出来的传感器数据，数据融合导航算法状态方程为：
其中，为系统状态，为系统噪声；
式(7)具体表示如下：
其中，系统状态包括：3维站心坐标系下东北天位置3维东北天地理系下速度4维姿态四元数3维陀螺零偏和3维加速度计零偏共16维；系统噪声包括：3维陀螺仪白噪声3维加速度计白噪声3维陀螺零偏随机游走噪声和3维加速度计零偏随机游走噪声共12维；为四元数的乘法矩阵；为四元数构成的载体坐标系到导航坐标系的坐标转换矩阵；为IMU中加速度计输出加速度(比力)、为IMU中陀螺仪输出角速度；根据状态微分方程(8)求解状态一步预测。
导航算法数据融合的量测方程为：
其中，表示量测；为量测由状态量表示的非线性函数；表示量测噪声；量测噪声共10维，与10维量测量相对应：3维位置量测噪声、3维速度量测噪声、3维归一化后的磁力计输出量测噪声、1维高度量测噪声；
式(10)中，表示站心坐标系下的3维GNSS位置或3维视觉导航传感器位置(即载体系下的视觉有效图像帧间位移)、表示地理坐标系下的3维 GNSS速度或3维视觉导航传感器载体系速度、表示3维由磁力计输出归一化后的磁力计量测、H由式(1)得到的1维融合高度。
式(10)中，当GNSS的位置和速度作为量测，可以表示为：
式(10)中，当视觉导航传感器的位置和速度作为量测，可以表示为：
式(11)和(12)中，为归一化的当地地理坐标系下的地磁分量；PU为状态中的高度；为上一次融合了视觉导航传感器数据的导航解算的位置输出；将状态一步预测值代入式(11)或(12)，即得到量测预测值；
状态一步预测值通过解微分方程获得：
式(13)中为上一导航周期状态估计值，T代表导航周期；
状态转移矩阵Φ计算：
式(14)中I为单位矩阵；
系统噪声驱动矩阵Γ计算：
量测矩阵H计算：
式(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输出的经度、纬度和高度转化为站心坐标系下的位置量测方法如下：
a.将Home点的经度λ、纬度L和高度h转换到ECEF坐标系下的坐标为： (HomeECEFxHomeECEFy HomeECEFz)，其转换公式如下：
其中，RN为需要坐标转换的位置点的卯酉圈的曲率半径，e为旋转椭球扁率。
b.导航过程中，GNSS输出的经度λS、纬度LS和高度hS同样转换到ECEF坐标系下的坐标为：(SATECEFx SATECEFy SATECEFz)，其转换方法与步骤a相同，为将λS、LS、hS代入上式(17)，RN为本坐标转换位置点的卯酉圈的曲率半径；
c.根据步骤a和步骤b的结果，GNSS输出的经度λS、纬度LS和高度hS转换为站心坐标系下的位置，并将站心坐标系下的位置作为量测方程中的位置量测，位置量测公式如下：
式(18)中由Home点的经度λ和纬度L构成：
站心坐标系下的导航位置输出转化为地球球面坐标系下的经度、纬度和高度的方法如下：
a.将站心坐标系下导航位置输出转换为ECEF坐标系下的坐标[X Y Z]T，转换公式为：
其中，同式(20)，同样由Home点的经纬度构成；
b.然后根据三角函数关系及迭代计算将ECEF坐标转化成地球球面坐标系下的经度、纬度和高度。
当飞行过程中GNSS定位失效，则用视觉导航传感器替代GNSS功能，解算出的位置同样采用以上步骤可将其转换到地球球面坐标下的经度、纬度和高度。
(2)当起飞时GNSS定位无效，经过一段时间恢复有效，反推GNSS定位无效时Home点的经度、纬度和高度及其ECEF坐标。
当无人机起飞时Home点无GNSS定位，其地球球面坐标下的经度、纬度和高度未知情况下，导航系统依靠视觉传感器的位置与速度信息代替GNSS功能；在飞行过程中GNSS恢复定位时，根据无人机导航系统此时刻解算的站心坐标系下的位置以及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)中，转换矩阵由伪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
由于导航算法中高度指的是垂直高度，而激光测距传感器因为安装的测量方向与载体系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，将归一化后的地磁矢量转换到东北天地理坐标系：
其中，[mE1 mN1 mU1]T为通过姿态矩阵直接转换得到的东北天地理坐标系下地磁矢量；为四元数构成的载体坐标系到导航坐标系的姿态矩阵；
接着，利用式(4)中通过姿态矩阵直接转换得到的东北天地理坐标系下地磁矢量[mE1 mN1 mU1]T，重构出地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T：
最后，将式(5)中地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T，通过地磁偏角的补偿转换到东北天地理坐标系下的地磁矢量[mE mN mU]T：
其中，Mag_dec为以弧度为单位的地磁偏角，由经纬度查询得到。
进一步，基于站心坐标系下的，采用EKF融合经过传感器数据健康管理系统筛选出来的传感器数据，数据融合导航算法状态方程为：
其中，为系统状态，为系统噪声；
式(7)具体表示如下：
其中，系统状态包括：3维站心坐标系下东北天位置3维东北天地理系下速度4维姿态四元数3维陀螺零偏和3维加速度计零偏共16维；系统噪声包括：3维陀螺仪白噪声3维加速度计白噪声3维陀螺零偏随机游走噪声和3维加速度计零偏随机游走噪声共12维；为四元数的乘法矩阵；为四元数构成的载体坐标系到导航坐标系的坐标转换矩阵；为IMU中加速度计输出加速度(比力)、为IMU中陀螺仪输出角速度；根据状态微分方程(8)求解状态一步预测。
导航算法数据融合的量测方程为：
其中，表示量测；为量测由状态量表示的非线性函数；表示量测噪声；量测噪声共10维，与10维量测量相对应：3维位置量测噪声、3维速度量测噪声、3维归一化后的磁力计输出量测噪声、1维高度量测噪声；
式(10)中，表示站心坐标系下的3维GNSS位置或3维视觉导航传感器位置(即载体系下的视觉有效图像帧间位移)、表示地理坐标系下的3维 GNSS速度或3维视觉导航传感器载体系速度、表示3维由磁力计输出归一化后的磁力计量测、H由式(1)得到的1维融合高度。
式(10)中，当GNSS的位置和速度作为量测，可以表示为：
式(10)中，当视觉导航传感器的位置和速度作为量测，可以表示为：
式(11)和(12)中，为归一化的当地地理坐标系下的地磁分量；PU为状态中的高度；为上一次融合了视觉导航传感器数据的导航解算的位置输出；将状态一步预测值代入式(11)或(12)，即得到量测预测值；
状态一步预测值通过解微分方程获得：
式(13)中为上一导航周期状态估计值，T代表导航周期；
状态转移矩阵Φ计算：
式(14)中I为单位矩阵；
系统噪声驱动矩阵Γ计算：
量测矩阵H计算：
式(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输出的经度、纬度和高度转化为站心坐标系下的位置量测方法如下：
a.将Home点的经度λ、纬度L和高度h转换到ECEF坐标系下的坐标为： (HomeECEFxHomeECEFy HomeECEFz)，其转换公式如下：
其中，RN为需要坐标转换的位置点的卯酉圈的曲率半径，e为旋转椭球扁率。
b.导航过程中，GNSS输出的经度λS、纬度LS和高度hS同样转换到ECEF坐标系下的坐标为：(SATECEFx SATECEFy SATECEFz)，其转换方法与步骤a相同，为将λS、LS、hS代入上式(17)，RN为本坐标转换位置点的卯酉圈的曲率半径；
c.根据步骤a和步骤b的结果，GNSS输出的经度λS、纬度LS和高度hS转换为站心坐标系下的位置，并将站心坐标系下的位置作为量测方程中的位置量测，位置量测公式如下：
式(18)中由Home点的经度λ和纬度L构成：
站心坐标系下的导航位置输出转化为地球球面坐标系下的经度、纬度和高度的方法如下：
a.将站心坐标系下导航位置输出转换为ECEF坐标系下的坐标[X Y Z]T，转换公式为：
其中，同式(20)，同样由Home点的经纬度构成；
b.然后根据三角函数关系及迭代计算将ECEF坐标转化成地球球面坐标系下的经度、纬度和高度。
当飞行过程中GNSS定位失效，则用视觉导航传感器替代GNSS功能，解算出的位置同样采用以上步骤可将其转换到地球球面坐标下的经度、纬度和高度。
(2)当起飞时GNSS定位无效，经过一段时间恢复有效，反推GNSS定位无效时Home点的经度、纬度和高度及其ECEF坐标。
当无人机起飞时Home点无GNSS定位，其地球球面坐标下的经度、纬度和高度未知情况下，导航系统依靠视觉传感器的位置与速度信息代替GNSS功能；在飞行过程中GNSS恢复定位时，根据无人机导航系统此时刻解算的站心坐标系下的位置以及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)中，转换矩阵由伪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 (6)
1.一种基于站心坐标系的小型无人机无缝自主式导航算法，其特征在于：包括导航系统，所述导航系统包括一套主IMU、主磁力计和主气压高度计，一套备用IMU、备用磁力计和备用气压高度计，以及激光测距传感器，GNSS和视觉导航传感器；
还包括以下步骤：
S1：由导航计算机读取安装于无人机上的主IMU、主磁力计、主气压高度计、备用IMU、备用磁力计、备用气压高度计、激光测距传感器、GNSS以及视觉导航传感器的数据信息，并将读取的数据信息提供给传感器数据健康管理系统；
S2：传感器数据健康状态管理系统对步骤S1获得的所有数据信息进行筛选，筛选出有效传感器数据用于导航数据融合解算，并对气压高度和激光高度进行预处理，计算出两者融合高度，该融合高度将作为导航数据融合的高度量测；传感器数据健康状态管理系统还将判断出GNSS与视觉导航传感器是否处于数据有效工作状态，当传感器数据健康状态管理系统判断出GNSS定位有效时，进入步骤S3，当传感器数据健康状态管理系统判断出GNSS无定位，但视觉导航传感器数据有效时，进入步骤S4；
S3：当GNSS定位有效时，如果当前导航周期GNSS数据有更新，将GNSS输出的经纬度转换到以无人机Home点为原点的站心坐标系下，再结合GNSS输出的地理坐标系的速度、以及传感器数据健康状态管理系统筛选出的IMU数据、磁力计数据以及高度数据，通过扩展卡尔曼滤波进行数据融合，解算出无人机的位置、速度和姿态；如果当前导航周期GNSS数据没有更新则直接进行捷联纯惯性导航递推；
S4：当GNSS无定位，但视觉导航传感器数据有效时，如果当前导航周期视觉导航传感器数据有更新，则采用视觉导航传感器输出的载体坐标系下的位移和速度、以及传感器数据健康状态管理系统筛选出的IMU数据、磁力计数据以及高度数据，通过扩展卡尔曼滤波进行数据融合，解算出无人机的位置、速度和姿态；如果当前导航周期视觉导航传感器数据没有更新则直接进行捷联纯惯性导航递推。
2.如权利要求1所述的基于站心坐标系的小型无人机无缝自主式导航算法，其特征在于：步骤S2中有效传感器的筛选方法具体为：
对于主IMU和备用IMU、主磁力计和备用磁力计、主气压高度计和备用气压高度计筛选时，主备数据通过数值差异对比、噪声方差统计、数值输出范围、传感器自有健康参数，判断出各传感器数据是否有效，当主IMU/主磁力计/主气压高度计处于正常工作时，就采用主IMU/主磁力计/主气压高度计的数据进行后续导航算法的运算，当判定主IMU/主磁力计/主气压高度计为非正常工作状态时，采用备用IMU/备用磁力计/备用气压高度计的数据进行后续导航算法的运算；
激光测距传感器通过噪声方差统计、数值输出范围、传感器自有健康参数判断出激光测距传感器数据的可信度；
GNSS定位判断，根据接收机定位状态参数、DOP值、定位卫星数量判断GNSS是否有效定位；
视觉导航传感器工作状态判断，视觉导航传感器有光照强度和特征点强度的信息输出，根据光照强度、特征点强度和相对地面的飞行高度，与视觉导航传感器有效工作下的光照强度、特征点强度以及飞行高度范围对比，确定出视觉导航传感器是否处于有效工作状态。
3.如权利要求2所述的基于站心坐标系的小型无人机无缝自主式导航算法，其特征在于：步骤S2中，气压高度与激光测距高度计算出一个融合高度，作为EKF数据融合的量测信息，中对气压高度与激光测距高度进行融合以计算融合高度的过程如下：
(1)确定初始高度HBaro0
在导航初始时刻，将此时气压高度计输出的气压高度，记为初始气压高度HBaro_T0，将此时激光测距传感器输出激光测距高度，记为初始激光测距高度HLaser_T0，计算用于气压高度变化计算的初始高度HBaro0，并忽略导航初始时刻载体水平姿态角导致的激光测距高度的非垂直性，初始高度HBaro0的计算公式如下：
HBaro0＝HBaro_T0-HLaser_T0 (1)
(2)确定激光测距传感器垂直高度HLaser
将激光测距传感器输出的激光测距高度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值越大，而激光测距传感器的健康参数实际上通过对传感器输出的激光测量距离值的噪声方差统计及超量程距离值个数统计共同确定。
4.如权利要求3所述的一种基于站心坐标系的小型无人机无缝自主式导航算法，其特征在于：在步骤S3和S4的导航数据融合中都存在磁力计数据的处理过程，即东北天地理坐标系下地磁矢量[mE mN mU]T的计算，其计算方法如下：
将载体坐标系下磁力计原始输出的归一化值[mx my mz]T转换到东北天地理坐标系：
其中，[mE1 mN1 mU1]T为通过姿态矩阵直接转换得到的东北天地理坐标系下地磁矢量；为四元数构成的载体坐标系到导航坐标系的姿态矩阵；
利用式(4)中东北天地理坐标系下地磁矢量[mE1 mN1 mU1]T，重构出地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T：
将式(5)中地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T，通过地磁偏角的补偿转换到东北天地理坐标系下的地磁矢量[mE mN mU]T：
其中，Mag_dec为以弧度为单位的地磁偏角，由经纬度查询得到。
5.如权利要求4所述的一种基于站心坐标系的小型无人机无缝自主式导航算法，其特征在于：基于站心坐标系下的，采用EKF融合经过传感器数据健康管理系统筛选出来的传感器数据，数据融合导航算法状态方程为：
其中，为系统状态，为系统噪声；
式(7)具体表示如下：
其中，系统状态包括：3维站心坐标系下东北天位置3维东北天地理系下速度4维姿态四元数3维陀螺零偏和3维加速度计零偏共16维；系统噪声包括：3维陀螺仪白噪声3维加速度计白噪声3维陀螺零偏随机游走噪声和3维加速度计零偏随机游走噪声共12维；为四元数的乘法矩阵；为四元数构成的载体坐标系到导航坐标系的坐标转换矩阵；为IMU中加速度计输出加速度(比力)、为IMU中陀螺仪输出角速度；根据状态微分方程(8)求解状态一步预测；
导航算法数据融合的量测方程为：
其中，表示量测；为量测由状态量表示的非线性函数；表示量测噪声；量测噪声共10维，与10维量测量相对应：3维位置量测噪声、3维速度量测噪声、3维归一化后的磁力计输出量测噪声、1维高度量测噪声；
式(10)中，表示站心坐标系下的3维GNSS位置或3维视觉导航传感器位置、表示地理坐标系下的3维GNSS速度或3维视觉导航传感器载体系速度、表示3维由磁力计输出归一化后的磁力计量测、H由式(1)得到的1维融合高度；
式(10)中，当GNSS的位置和速度作为量测，可以表示为：
式(10)中，当视觉导航传感器的位置和速度作为量测，可以表示为：
式(11)和(12)中，为归一化的当地地理坐标系下的地磁分量；PU为状态中的高度；为上一次融合了视觉导航传感器数据的导航解算的位置输出；将状态一步预测值代入式(11)或(12)，即得到量测预测值；
状态一步预测值通过解微分方程获得：
式(13)中为上一导航周期状态估计值，T代表导航周期；
状态转移矩阵Φ计算：
式(14)中I为单位矩阵；
系统噪声驱动矩阵Γ计算：
量测矩阵H计算：
式(16)中，GNSS参与导航时对应式(11)用于计算量测矩阵，当视觉导航传感器参与导航时对应式(12)用于计算量测矩阵。
6.如权利要求5所述的一种基于站心坐标系的小型无人机无缝自主式导航算法，其特征在于：步骤S3和S4中，涉及相关传感器输出数据需要进行坐标的转换，位置量测和位置估计的坐标转换：
(1)当无人机起飞时及后续飞行过程中，GNSS定位一直处于有效环境下，所需要的坐标转换如下：
站心坐标系原点为无人机起始点，即Home点，其坐标为(0 0 0)，在GNSS定位有效环境下，通过GNSS获取Home点经度、纬度和高度，分别记为λ、L、h；导航过程中，GNSS输出的经度、纬度和高度分别记为λS、LS和hS，将导航过程中，GNSS输出的经度、纬度和高度转化为站心坐标系下的位置量测方法如下：
a.将Home点的经度λ、纬度L和高度h转换到ECEF坐标系下，其坐标记为：(HomeECEFxHomeECEFy HomeECEFz)；
b.导航过程中，GNSS输出的经度λS、纬度LS和高度hS同样转换到ECEF坐标系下，其坐标记为：(SATECEFx SATECEFy SATECEFz)；
c.根据步骤a和步骤b的结果，GNSS输出的经度λS、纬度LS和高度hS转换为站心坐标系下的位置，并将站心坐标系下的位置作为量测方程中的位置量测，位置量测公式如下：
式(18)中由Home点的经度λ和纬度L构成：
然后，将站心坐标系下的导航位置输出转化为地球球面坐标系下的经度、纬度和高度，其转化方法如下：
a.将站心坐标系下导航位置输出转换为ECEF坐标系下的坐标[X Y Z]T，转换公式为：
其中，同式(20)，同样由Home点的经纬度构成；
b.然后根据三角函数关系及迭代计算将ECEF坐标转化成地球球面坐标系下的经度、纬度和高度；
(2)当起飞时GNSS定位无效，经过一段时间恢复有效，反推GNSS定位无效时Home点的经度、纬度和高度及其ECEF坐标的过程如下：
当无人机起飞时Home点无GNSS定位，其地球球面坐标系下的经度、纬度和高度未知情况下，导航系统依靠视觉传感器的位置与速度信息代替GNSS功能；在飞行过程中GNSS恢复定位时，根据无人机导航系统此时刻解算的站心坐标系下的位置以及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)中，转换矩阵由伪Home点的经度λS和纬度LS构成，Home点的ECEF坐标由伪Home点的ECEF坐标(SATECEFx SATECEFy SATECEFz)替代；
d.得到Home点的ECEF坐标后即可通过三角函数关系及迭代计算得到真实Home点的球面坐标经度λ、纬度L和高度h；
GNSS恢复定位后，完成Home点球面坐标及ECEF坐标的反推，依然根据公式(18)计算GNSS输出的位置量测。
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 true CN109931926A (zh) | 2019-06-25 |
| CN109931926B CN109931926B (zh) | 2023-04-25 |
Family
ID=66989303
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910270662.3A Active CN109931926B (zh) | 2019-04-04 | 2019-04-04 | 一种基于站心坐标系的小型无人机无缝自主式导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109931926B (zh) |
Cited By (17)
| 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 | 苏州博田自动化技术有限公司 | 基于控制时序组合的宏微导航方法 |
| CN111207734A (zh) * | 2020-01-16 | 2020-05-29 | 西安因诺航空科技有限公司 | 一种基于ekf的无人机组合导航方法 |
| CN111445491A (zh) * | 2020-03-24 | 2020-07-24 | 山东智翼航空科技有限公司 | 微型无人机三邻域极大差值边缘检测狭道导引算法 |
| CN111486841A (zh) * | 2020-05-19 | 2020-08-04 | 西安因诺航空科技有限公司 | 一种基于激光定位系统的无人机导航定位方法 |
| CN112558087A (zh) * | 2020-11-20 | 2021-03-26 | 东风汽车集团有限公司 | 一种自动驾驶车辆的定位系统及方法 |
| CN112634354A (zh) * | 2020-12-21 | 2021-04-09 | 紫清智行科技(北京)有限公司 | 基于路侧传感器的网联自动驾驶行车风险评估方法与装置 |
| CN113177309A (zh) * | 2021-04-23 | 2021-07-27 | 合肥赛为智能有限公司 | 一种基于决策算法的无人机冗余传感器的数据分析方法 |
| CN113237478A (zh) * | 2021-05-27 | 2021-08-10 | 哈尔滨工业大学 | 一种无人机的姿态和位置估计方法及无人机 |
| CN113325454A (zh) * | 2021-05-18 | 2021-08-31 | 武汉大学 | 一种基于ArduRover无人车的组合定位方法 |
| CN113821059A (zh) * | 2021-11-24 | 2021-12-21 | 中航金城无人系统有限公司 | 一种多旋翼无人机传感器故障安全飞行控制系统及方法 |
| CN114115010A (zh) * | 2021-11-18 | 2022-03-01 | 中国特种飞行器研究所 | 一种用于无人热气球的飞行高度控制系统 |
| CN114625155A (zh) * | 2022-05-13 | 2022-06-14 | 向量传感科技(宁波)有限公司 | 自动驾驶软件三重冗余管理系统及方法 |
| CN115307626A (zh) * | 2021-05-06 | 2022-11-08 | 北京航通云科技有限公司 | 一种应用于小型无人机的冗余定位方法 |
| CN116448505A (zh) * | 2023-06-16 | 2023-07-18 | 生态环境部华南环境科学研究所(生态环境部生态环境应急研究所) | 基于无人船重力测量的水质检测自动采样方法及系统 |
| CN117148406A (zh) * | 2023-10-30 | 2023-12-01 | 山东大学 | 一种室内外无缝弹性融合定位方法、系统、介质及设备 |
| CN118519180A (zh) * | 2024-06-13 | 2024-08-20 | 比亚迪股份有限公司 | 导航定位方法、装置、介质和车辆 |
Citations (26)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20100283832A1 (en) * | 2004-08-04 | 2010-11-11 | American Gnc Corporation | Miniaturized GPS/MEMS IMU integrated board |
| CN102506857A (zh) * | 2011-11-28 | 2012-06-20 | 北京航空航天大学 | 一种基于双imu/dgps组合的相对姿态测量实时动态滤波方法 |
| CN103344218A (zh) * | 2013-06-18 | 2013-10-09 | 桂林理工大学 | 一种适用于低空无人机的高度测量系统及方法 |
| CN103697889A (zh) * | 2013-12-29 | 2014-04-02 | 北京航空航天大学 | 一种基于多模型分布式滤波的无人机自主导航与定位方法 |
| 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 |
| CN104697520A (zh) * | 2015-02-05 | 2015-06-10 | 南京航空航天大学 | 一体化无陀螺捷联惯导系统与gps系统组合导航方法 |
| CN104729506A (zh) * | 2015-03-27 | 2015-06-24 | 北京航空航天大学 | 一种视觉信息辅助的无人机自主导航定位方法 |
| 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 |
| CN105571591A (zh) * | 2015-12-15 | 2016-05-11 | 中国电子科技集团公司第二十六研究所 | 一种多信息深组合导航微系统及导航方法 |
| CN105628024A (zh) * | 2015-12-29 | 2016-06-01 | 中国电子科技集团公司第二十六研究所 | 基于多传感器融合的单人定位导航器和定位导航方法 |
| CN106030430A (zh) * | 2013-11-27 | 2016-10-12 | 宾夕法尼亚大学理事会 | 用于使用旋翼微型航空载具（mav）在室内和室外环境中的稳健的自主飞行的多传感器融合 |
| CN106595653A (zh) * | 2016-12-08 | 2017-04-26 | 南京航空航天大学 | 一种穿戴式行人自主导航系统及其导航方法 |
| CN106780699A (zh) * | 2017-01-09 | 2017-05-31 | 东南大学 | 一种基于sins/gps和里程计辅助的视觉slam方法 |
| CN106950586A (zh) * | 2017-01-22 | 2017-07-14 | 无锡卡尔曼导航技术有限公司 | 用于农机作业的gnss/ins/车辆组合导航方法 |
| CN107024206A (zh) * | 2017-04-17 | 2017-08-08 | 重庆邮电大学 | 一种基于ggi/gps/ins的组合导航系统 |
| CN107289948A (zh) * | 2017-07-24 | 2017-10-24 | 成都通甲优博科技有限责任公司 | 一种基于多传感器融合的无人机导航系统及方法 |
| CN107478221A (zh) * | 2017-08-11 | 2017-12-15 | 黄润芳 | 一种用于移动终端的高精度定位方法 |
| US20180031387A1 (en) * | 2016-07-29 | 2018-02-01 | Carnegie Mellon University | State estimation for aerial vehicles using multi-sensor fusion |
| CN107747940A (zh) * | 2017-05-11 | 2018-03-02 | 南京继航科技有限公司 | 一种基于fpga及rtos的多传感器融合导航装置 |
| CN107907900A (zh) * | 2017-11-07 | 2018-04-13 | 长光卫星技术有限公司 | 一种gnss双天线辅助的多传感器组合导航系统及方法 |
| CN108375370A (zh) * | 2018-07-02 | 2018-08-07 | 江苏中科院智能科学技术应用研究院 | 一种面向智能巡防无人机的复合导航系统 |
| CN108957511A (zh) * | 2018-05-21 | 2018-12-07 | 永康威力科技股份有限公司 | 一种车辆导航转向控制系统和导航修正方法 |
| US10151588B1 (en) * | 2016-09-28 | 2018-12-11 | Near Earth Autonomy, Inc. | Determining position and orientation for aerial vehicle in GNSS-denied situations |
| CN109000640A (zh) * | 2018-05-25 | 2018-12-14 | 东南大学 | 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法 |
| CN109059904A (zh) * | 2018-06-01 | 2018-12-21 | 浙江亚特电器有限公司 | 用于移动载具的组合导航方法 |
| CN109387192A (zh) * | 2017-08-02 | 2019-02-26 | 湖南格纳微信息科技有限公司 | 一种室内外连续定位方法及装置 |
-
2019
- 2019-04-04 CN CN201910270662.3A patent/CN109931926B/zh active Active
Patent Citations (26)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20100283832A1 (en) * | 2004-08-04 | 2010-11-11 | American Gnc Corporation | Miniaturized GPS/MEMS IMU integrated board |
| CN102506857A (zh) * | 2011-11-28 | 2012-06-20 | 北京航空航天大学 | 一种基于双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 | 桂林理工大学 | 一种适用于低空无人机的高度测量系统及方法 |
| CN106030430A (zh) * | 2013-11-27 | 2016-10-12 | 宾夕法尼亚大学理事会 | 用于使用旋翼微型航空载具（mav）在室内和室外环境中的稳健的自主飞行的多传感器融合 |
| CN103697889A (zh) * | 2013-12-29 | 2014-04-02 | 北京航空航天大学 | 一种基于多模型分布式滤波的无人机自主导航与定位方法 |
| 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 |
| CN104697520A (zh) * | 2015-02-05 | 2015-06-10 | 南京航空航天大学 | 一体化无陀螺捷联惯导系统与gps系统组合导航方法 |
| CN104729506A (zh) * | 2015-03-27 | 2015-06-24 | 北京航空航天大学 | 一种视觉信息辅助的无人机自主导航定位方法 |
| CN105571591A (zh) * | 2015-12-15 | 2016-05-11 | 中国电子科技集团公司第二十六研究所 | 一种多信息深组合导航微系统及导航方法 |
| CN105628024A (zh) * | 2015-12-29 | 2016-06-01 | 中国电子科技集团公司第二十六研究所 | 基于多传感器融合的单人定位导航器和定位导航方法 |
| US20180031387A1 (en) * | 2016-07-29 | 2018-02-01 | 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 | 南京航空航天大学 | 一种穿戴式行人自主导航系统及其导航方法 |
| CN106780699A (zh) * | 2017-01-09 | 2017-05-31 | 东南大学 | 一种基于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 | 成都通甲优博科技有限责任公司 | 一种基于多传感器融合的无人机导航系统及方法 |
| CN109387192A (zh) * | 2017-08-02 | 2019-02-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 | 永康威力科技股份有限公司 | 一种车辆导航转向控制系统和导航修正方法 |
| CN109000640A (zh) * | 2018-05-25 | 2018-12-14 | 东南大学 | 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法 |
| CN109059904A (zh) * | 2018-06-01 | 2018-12-21 | 浙江亚特电器有限公司 | 用于移动载具的组合导航方法 |
| CN108375370A (zh) * | 2018-07-02 | 2018-08-07 | 江苏中科院智能科学技术应用研究院 | 一种面向智能巡防无人机的复合导航系统 |
Cited By (25)
| 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的无人机组合导航方法 |
| CN111207734A (zh) * | 2020-01-16 | 2020-05-29 | 西安因诺航空科技有限公司 | 一种基于ekf的无人机组合导航方法 |
| CN111445491A (zh) * | 2020-03-24 | 2020-07-24 | 山东智翼航空科技有限公司 | 微型无人机三邻域极大差值边缘检测狭道导引算法 |
| CN111445491B (zh) * | 2020-03-24 | 2023-09-15 | 山东智翼航空科技有限公司 | 微型无人机三邻域极大差值边缘检测狭道导引方法 |
| CN111486841A (zh) * | 2020-05-19 | 2020-08-04 | 西安因诺航空科技有限公司 | 一种基于激光定位系统的无人机导航定位方法 |
| CN111486841B (zh) * | 2020-05-19 | 2022-04-05 | 西安因诺航空科技有限公司 | 一种基于激光定位系统的无人机导航定位方法 |
| CN112558087A (zh) * | 2020-11-20 | 2021-03-26 | 东风汽车集团有限公司 | 一种自动驾驶车辆的定位系统及方法 |
| CN112558087B (zh) * | 2020-11-20 | 2023-06-23 | 东风汽车集团有限公司 | 一种自动驾驶车辆的定位系统及方法 |
| CN112634354A (zh) * | 2020-12-21 | 2021-04-09 | 紫清智行科技(北京)有限公司 | 基于路侧传感器的网联自动驾驶行车风险评估方法与装置 |
| CN113177309A (zh) * | 2021-04-23 | 2021-07-27 | 合肥赛为智能有限公司 | 一种基于决策算法的无人机冗余传感器的数据分析方法 |
| CN115307626A (zh) * | 2021-05-06 | 2022-11-08 | 北京航通云科技有限公司 | 一种应用于小型无人机的冗余定位方法 |
| CN113325454A (zh) * | 2021-05-18 | 2021-08-31 | 武汉大学 | 一种基于ArduRover无人车的组合定位方法 |
| CN113325454B (zh) * | 2021-05-18 | 2022-06-14 | 武汉大学 | 一种基于ArduRover无人车的组合定位方法 |
| CN113237478A (zh) * | 2021-05-27 | 2021-08-10 | 哈尔滨工业大学 | 一种无人机的姿态和位置估计方法及无人机 |
| CN114115010A (zh) * | 2021-11-18 | 2022-03-01 | 中国特种飞行器研究所 | 一种用于无人热气球的飞行高度控制系统 |
| CN113821059A (zh) * | 2021-11-24 | 2021-12-21 | 中航金城无人系统有限公司 | 一种多旋翼无人机传感器故障安全飞行控制系统及方法 |
| CN114625155A (zh) * | 2022-05-13 | 2022-06-14 | 向量传感科技(宁波)有限公司 | 自动驾驶软件三重冗余管理系统及方法 |
| CN114625155B (zh) * | 2022-05-13 | 2022-08-19 | 向量传感科技(宁波)有限公司 | 自动驾驶软件三重冗余管理系统及方法 |
| CN116448505A (zh) * | 2023-06-16 | 2023-07-18 | 生态环境部华南环境科学研究所(生态环境部生态环境应急研究所) | 基于无人船重力测量的水质检测自动采样方法及系统 |
| CN116448505B (zh) * | 2023-06-16 | 2023-09-01 | 生态环境部华南环境科学研究所(生态环境部生态环境应急研究所) | 基于无人船重力测量的水质检测自动采样方法及系统 |
| CN117148406A (zh) * | 2023-10-30 | 2023-12-01 | 山东大学 | 一种室内外无缝弹性融合定位方法、系统、介质及设备 |
| CN117148406B (zh) * | 2023-10-30 | 2024-01-30 | 山东大学 | 一种室内外无缝弹性融合定位方法、系统、介质及设备 |
| CN118519180A (zh) * | 2024-06-13 | 2024-08-20 | 比亚迪股份有限公司 | 导航定位方法、装置、介质和车辆 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109931926B (zh) | 2023-04-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109931926B (zh) | 一种基于站心坐标系的小型无人机无缝自主式导航方法 | |
| CN110274588B (zh) | 基于无人机集群信息的双层嵌套因子图多源融合导航方法 | |
| CN107655476B (zh) | 基于多信息融合补偿的行人高精度足部导航方法 | |
| US8577595B2 (en) | Location and path-map generation data acquisition and analysis systems | |
| CN106500693B (zh) | 一种基于自适应扩展卡尔曼滤波的ahrs算法 | |
| CN109916394A (zh) | 一种融合光流位置和速度信息的组合导航算法 | |
| CN112697138B (zh) | 一种基于因子图优化的仿生偏振同步定位与构图的方法 | |
| WO2014134710A1 (en) | Method and apparatus for fast magnetometer calibration | |
| CN109916395A (zh) | 一种姿态自主冗余组合导航算法 | |
| CN112505737A (zh) | 一种基于Elman神经网络在线学习辅助的GNSS/INS组合导航方法 | |
| CN114018242B (zh) | 一种基于偏振/太阳/惯性信息智能匹配的自主定姿方法 | |
| CN104181573A (zh) | 北斗惯导深组合导航微系统 | |
| CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
| CN112697154B (zh) | 一种基于矢量分配的自适应多源融合导航方法 | |
| CN113834483A (zh) | 一种基于可观测度的惯性/偏振/地磁容错导航方法 | |
| Gao et al. | An integrated land vehicle navigation system based on context awareness | |
| CN114964221B (zh) | 基于rtk/uwb/imu多单元协同室内外定位方法 | |
| CN115856922A (zh) | 一种松耦合陆地组合导航方法、装置、计算机设备和介质 | |
| Xiang et al. | A SINS/GNSS/2D-LDV integrated navigation scheme for unmanned ground vehicles | |
| Nebot et al. | Initial calibration and alignment of an inertial navigation | |
| CN115356965B (zh) | 一种松散耦合实装数据采集装置及数据处理方法 | |
| RU2277696C2 (ru) | Интегрированная инерциально-спутниковая навигационная система | |
| CN112923934A (zh) | 一种适用于非结构化场景中结合惯导的激光slam技术 | |
| CN112649001B (zh) | 一种小型无人机姿态与位置解算方法 | |
| CN115479605A (zh) | 基于空间目标定向观测的高空长航时无人机自主导航方法 |
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 |