CN109916394A - 一种融合光流位置和速度信息的组合导航算法 - Google Patents
一种融合光流位置和速度信息的组合导航算法 Download PDFInfo
- Publication number
- CN109916394A CN109916394A CN201910270669.5A CN201910270669A CN109916394A CN 109916394 A CN109916394 A CN 109916394A CN 201910270669 A CN201910270669 A CN 201910270669A CN 109916394 A CN109916394 A CN 109916394A
- Authority
- CN
- China
- Prior art keywords
- carrier
- light stream
- formula
- coordinate system
- height
- 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.)
- Pending
Links
- 230000003287 optical effect Effects 0.000 title claims abstract description 22
- 230000004927 fusion Effects 0.000 claims abstract description 60
- 238000006073 displacement reaction Methods 0.000 claims description 70
- 238000005259 measurement Methods 0.000 claims description 32
- 239000011159 matrix material Substances 0.000 claims description 31
- 241000209094 Oryza Species 0.000 claims description 12
- 235000007164 Oryza sativa Nutrition 0.000 claims description 12
- 230000001133 acceleration Effects 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 12
- 235000009566 rice Nutrition 0.000 claims description 12
- 230000005389 magnetism Effects 0.000 claims description 11
- 238000000034 method Methods 0.000 claims description 10
- 238000006243 chemical reaction Methods 0.000 claims description 9
- 238000009434 installation Methods 0.000 claims description 9
- 238000010606 normalization Methods 0.000 claims description 5
- 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 3
- 230000000737 periodic effect Effects 0.000 claims description 3
- 238000006467 substitution reaction Methods 0.000 claims description 2
- 229910017435 S2 In Inorganic materials 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
本发明提供一种融合光流位置和速度信息的组合导航算法，利用光流传感器输出的位置和速度信息以及MEMS IMU、磁力计、气压高度计、激光测距传感器的数据，采用扩展卡尔曼滤波器完成数据融合，解算出载体的位置、速度和姿态信息。本算法采用站心坐标系作为导航坐标系，实时解算出载体相对初始位置点的位置坐标。本算法可以实现载体在GNSS拒止条件下的导航与定位，为载体提供准确的航向、姿态和速度信息，并有效减缓捷联纯惯性定位误差的漂移速率。
Description
技术领域
本发明涉及组合导航算法技术领域，特别是涉及一种融合光流位置和速度信息的组合导航算法。
背景技术
基于低精度的MEMS IMU的惯性导航无法为载体提供可用导航数据，常采用GNSS数据来对抑制惯导误差的发散，但GNSS在室内、城市侠道、桥梁及隧道等环境下无法提供定位信息，在这些GNSS拒止环境下，需要寻找伪GNSS来对惯导进行辅助，所谓伪GNSS，即在GNSS拒止环境下，其在功能上部分或全部替代GNSS实现速度或位置的测量，从而与惯导及其它传感器数据进行融合，实现GNSS拒止环境下的导航与定位功能。
发明内容
本发明所要解决的技术问题是：为了克服GNSS拒止环境下导航定位问题，本发明提供一种融合光流位置和速度信息的组合导航算法，本算法融合了光流传感器、MEMS IMU、磁力计、气压高度计、激光测距传感器的数据信息，采用扩展卡尔曼滤波完成数据融合，在站心坐标系下解算出载体的位置、速度和姿态信息。
本发明解决其技术问题所要采用的技术方案是：
本发明所涉及的组合导航***包括安装于载体上的光流传感器、MEMS IMU(简称IMU)、磁力计、气压高度计和激光测距传感器，光流传感器相机坐标系、IMU坐标系、磁力计坐标系与右前上载体坐标系重合，光流传感器与激光测距传感器安装于载体底部，激光测距传感器测量方向与载体上轴方向相反，其中，
IMU包含三轴正交陀螺仪和三轴正交加速度计，分别用于测量角速度和加速度(比力)；磁力计采用三轴正交磁力计，用于地磁测量；气压高度计用于测量气压高度；光流传感器用于测量像素坐标系下两帧有效图像之间的像素位移；激光测距传感器用于测量传感器和反射点之间的单维距离。
本发明涉及到的坐标系包括载体坐标系、站心坐标系、东北天地理坐标系、导航坐标系、地磁东北天坐标系、相机坐标系、像素坐标系，其中，载体坐标系指以装载导航***的载体的“右前上”坐标系，安装于载体上的具有坐标的传感器(IMU、磁力计、光流传感器)其安装与载体坐标系重合；站心坐标系为载体导航起始点为原点的东北天地理坐标系，组合导航的位置估计以站心坐标系表示；东北天地理坐标系的原点为载体质心，组合导航的速度估计表示在本坐标系中，且认为等同于表示于站心坐标系中；在本算法中导航坐标系采用站心坐标系；地磁东北天坐标系以磁北为北向，东北天地理坐标系天向轴旋转一个地磁偏角即与地磁东北天坐标系重合，相机坐标系即相机的“右前上”坐标系，像素坐标系原点为图像左上角，单位为像素(pixel),u轴与相机坐标系的右向轴方向相同，v轴与相机坐标系的前向轴方向相同。
一种融合光流位置和速度信息的组合导航算法，包括以下步骤：
S1：由导航计算机读取安装于载体上的光流传感器、IMU、磁力计、气压高度计和激光测距传感器的数据信息，其中，从光流传感器读取的是像素坐标系下u轴方向和v轴方向的两帧有效图像之间的像素位移，从IMU上读取的是角速度和加速度(比力)数据，从磁力计上读取的是地磁强度数据，从气压高度计上读取的是气压高度数据，从激光测距传感器上读取的是激光测距高度数据；
S2：采用变权值法对步骤S1中获取的数据信息中的气压高度与激光测距高度进行融合，计算获得一个融合高度，并将融合高度作为扩展卡尔曼滤波(EKF)数据融合的高度量测；
S3：导航算法根据光流传感器数据更新标志，判断光流传感器数据是否更新，如果光流传感器数据没有更新，进入步骤S4；如果光流传感器数据有更新，则进入步骤S5；
S4：在光流传感器数据无更新情况下，导航算法进行捷联纯惯性导航递推，解算出载体的位置、速度和姿态信息；
S5：在光流传感器数据有更新情况下，对光流传感器输出的两维像素位移进行角运动补偿，得到线运动对应的像素位移，并利用相机分辨率和相机与被拍摄平面之间的物理距离对线运动像素位移进行物理尺度转换，将线运动像素位移转化为米为单位的相机坐标系下右向和前向位移；并根据两帧有效光流传感器数据输出之间的间隔时间计算出相机坐标系下右向和前向速度；通过安装确保光流传感器与载体坐标系重合，因此相机坐标系下右向和前向的位移及速度，即载体在载体坐标系下右向和前向的位移和速度；
S6：根据步骤S5的结果，通过扩展卡尔曼滤波对光流传感器数据计算得到的载体系位置(即位移)和速度、IMU数据、磁力计数据、融合高度进行数据融合，解算出载体的位置、速度和姿态信息。
在步骤S2中，气压高度与激光测距高度计算出一个融合高度，作为扩展卡尔曼滤波EKF数据融合的量测信息，对气压高度与激光测距高度进行融合以计算融合高度的过程如下：
(1)确定初始高度HBaro0
在导航初始时刻，将此时气压高度计输出的气压高度，记为初始气压高度HBaro_T0，将此时激光测距传感器输出激光测距高度，记为初始激光测距高度HLaser_T0，计算用于气压高度变化计算的初始高度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值越大，而激光测距传感器的健康参数实际上通过对传感器输出的激光测量距离值的噪声方差统计及超量程距离值个数统计共同确定。
进一步，步骤S5中对光流传感器输出的光流传感器数据，即像素坐标系u轴方向和v轴方向的像素位移，进行角运动补偿和物理尺度转换的过程具体如下：
本算法中，光流传感器采用底部安装，光流传感器的相机坐标系、IMU坐标系、磁力计坐标系与“右前上”载体坐标系保持安装一致，像素坐标系的u轴和载体坐标系的右向轴平行，像素坐标系的v轴与载体坐标系前向轴平行；所述光流传感器直接输出像素坐标系下的u轴像素位移OpFlowX和v轴像素位移OpFlowY，u轴像素位移OpFlowX和v轴像素位移OpFlowY包含线运动产生的像素位移和角运动产生的像素位移，为提取出线运动信息，需要补偿载体角运动产生的像素位移：
式(4)中，OpFlowX_transmotion、OpFlowY_transmotion为线运动产生的像素位移；γlast、θlast为上一有效光流传感器数据输出时的以弧度为单位的载体横滚角和俯仰角；K为光流传感器角运动补偿参数，K值为光流传感器手册中给出的一项传感器出厂参数，也可使光流传感器在固定高度下只做角运动测试得到。
得到(4)式中的线运动产生的像素位移后，将其转化为以米为单位的相机坐标系位移：
式(5)中OpFlowPx、OpFlowPy为两帧有效光流传感器数据之间的光流传感器在相机坐标系下右向和前向的位移，单位为米，因为，相机坐标系与载体坐标系重合，因此，该位移也是光流传感器在载体坐标系下的位移；resolution为光流传感器的分辨率，可以通过查光流传感器的手册获得，也可通过线运动下GPS速度标定、加速度计输出的加速度积分后的速度进行标定或者沿固定距离之间往复直线运动标定等方式得到该参数；PU为组合导航数据融合得到的高度。
得到(5)式中米为单位的光流传感器在载体坐标系下位移后，计算出米/秒为单位的光流传感器在载体系下的速度：
式(6)中，OpFlowVx、OpFlowVy为光流传感器在载体坐标系的右向和前向的速度；tnow为光流传感器当前帧输出时秒为单位的时间；tlast为光流传感器上一帧数据输出时秒为单位的时间。
进一步，步骤S6中，在进行导航数据融合时，涉及磁力计数据的处理，归一化的当地地理坐标系(东北天)下的地磁矢量[mE mN mU]T的计算方法为：
将载体坐标系下，磁力计原始输出的归一化值[mx my mz]T转换到地理坐标系：
其中，[mE1 mN1 mU1]T为通过姿态矩阵直接转换得到的地理坐标系下地磁矢量；为四元数构成的载体坐标系到导航坐标系的姿态矩阵。
利用式(7)中地理坐标系下地磁矢量[mE1 mN1 mU1]T，重构出地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T：
将式(8)中地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T，通过地磁偏角的补偿转换到地理坐标系下的地磁矢量[mE mN mU]T：
其中，Mag_dec为弧度为单位的地磁偏角，由经纬度查询得到。
基于站心坐标系下的，采用EKF融合光流传感器位置和速度的组合导航算法，组合导航算法状态方程为：
其中，为***状态，为***噪声；包括：3维位置3维速度4维姿态四元数3维陀螺零偏和3维加速度计零偏共16维；***噪声包括：3维陀螺仪白噪声3维加速度计白噪声共6维；为四元数的乘法矩阵；为IMU中加速度计输出加速度、为IMU中陀螺仪输出角速度；根据状态微分方程(10)求解状态一步预测。
组合导航算法量测方程：
其中，表示量测；为量测预测值；表示量测噪声；
式(11)中，量测噪声包括2维光流位移量测噪声、2维光流速度量测噪声、3维归一化后的磁力计输出量测噪声、1维融合高度量测噪声。
式(11)中，表示8维量测，可以用下式(12)表示：
式(12)中，表示2维光流传感器输出载体系右向和前向位移OpFlowPx与OpFlowPy、表示2维光流传感器输出载体系右向和前向速度OpFlowVx与OpFlowVy、表示3维由磁力计输出归一化后的磁力计量测、H表示1维气压高度和激光测距高度的融合高度。
式(11)中，为量测与状态非线性关系函数，可以表示为：
式(13)中和由式(14)和式(15)计算得到：
其中，为状态中东北天位置转化到载体系的右向和前向位移，即式(14)中载体系速度[Px Py Pz]T中的Px和Py；为状态中东北天速度转换到载体系的右向和前向速度，即式(15)中载体系速度[Vx Vy Vz]T中的Vx和Vy；PU即状态中的高度；为上一次光流数据有效时导航数据融合的东北天位置状态估计；为归一化的当地地理坐标系(东北天)下的地磁矢量；将状态一步预测值代入式(13)，即得到量测预测值。
式(11)中，量测噪声包括2维光流位移量测噪声、2维光流速度量测噪声、3维归一化后的磁力计输出量测噪声、1维融合高度量测噪声。
对式(10)和式(13)，通过计算雅克比矩阵，得到状态转移矩阵Φ、***噪声驱动矩阵Γ以及量测矩阵H。
状态转移矩阵Φ计算：
***噪声驱动矩阵Γ计算：
式(16)、(17)中T为导航周期，I为单位矩阵；
量测矩阵H计算：
状态一步预测值通过解微分方程获得：
式(19)中为上一导航周期状态估计值；
然后利用扩展卡尔曼滤波即可完成数据融合，解算出载体的位置、速度和姿态信息。
本发明的有益效果是：本发明提供的一种融合光流位置和速度信息的组合导航算法，利用光流传感器输出的位置和速度信息以及MEMS IMU、磁力计、气压高度计、激光测距传感器的数据，采用扩展卡尔曼滤波器完成数据融合，解算出载体的位置、速度和姿态信息。本算法采用站心坐标系作为导航坐标系，实时解算出载体相对初始位置点的位置坐标。本算法可以实现载体在GNSS拒止条件下的导航与定位，为载体提供准确的航向姿态和速度测量数据，并有效减缓捷联纯惯性定位误差的漂移速率。
附图说明
下面结合附图和实施例对本发明作进一步说明。
图1是本发明算法的原理框图。
图2是本发明的算法流程图。
具体实施方式
现在结合附图对本发明作详细的说明。此图为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。
如图1所示，本发明所涉及的组合导航***包括安装于载体上的光流传感器、MEMSIMU(简称IMU)、磁力计、气压高度计和激光测距传感器，光流传感器相机坐标系、IMU坐标系、磁力计坐标系与右前上载体坐标系重合，其中，
IMU包含三轴正交陀螺仪和三轴正交加速度计，分别用于测量角速度和加速度(比力)；磁力计采用三轴正交磁力计，用于地磁测量；气压高度计用于测量气压高度；光流传感器用于测量像素坐标系下两帧有效图像之间的像素位移；激光测距传感器用于测量传感器和反射点之间的单维距离。
如图2所示，一种融合光流位置和速度信息的组合导航算法，包括以下步骤：
S1：由导航计算机读取安装于载体上的光流传感器、IMU、磁力计、气压高度计和激光测距传感器的数据信息，其中，从光流传感器读取的是像素坐标系下u轴方向和v轴方向的两帧有效图像之间的像素位移，从IMU上读取的是角速度和加速度(比力)数据，从磁力计上读取的是地磁强度数据，从气压高度计上读取的是气压高度数据，从激光测距传感器上读取的是激光测距高度数据；
S2：采用变权值法对步骤S1中获取的数据信息中的气压高度与激光测距高度进行融合，计算获得一个融合高度，并将融合高度作为扩展卡尔曼滤波(EKF)数据融合的高度量测；
S3：导航算法根据光流传感器数据更新标志，判断光流传感器数据是否更新，如果光流传感器数据没有更新，进入步骤S4；如果光流传感器数据有更新，则进入步骤S5；
S4：在光流传感器数据无更新情况下，导航算法进行捷联纯惯性导航递推，解算出载体的位置、速度和姿态信息；
S5：在光流传感器数据有更新情况下，对光流传感器输出的两维像素位移进行角运动补偿，得到线运动对应的像素位移，并利用相机分辨率和相机与被拍摄平面之间的物理距离对线运动像素位移进行物理尺度转换，将线运动像素位移转化为米为单位的相机坐标系下右向和前向位移；并根据两帧有效光流传感器数据输出之间的间隔时间计算出相机坐标系下右向和前向速度；通过安装确保光流传感器与载体坐标系重合，因此相机坐标系下右向和前向的位移及速度，即载体在载体坐标系下右向和前向的位移和速度；
S6：根据步骤S5的结果，通过扩展卡尔曼滤波对光流传感器数据计算得到的载体系位置(即位移)和速度、IMU数据、磁力计数据、融合高度进行数据融合，解算出载体的位置、速度和姿态信息。
在步骤S2中，气压高度与激光测距高度计算出一个融合高度，作为扩展卡尔曼滤波EKF数据融合的量测信息，中对气压高度与激光测距高度进行融合以计算融合高度的过程如下：
(1)确定初始高度HBaro0
在导航初始时刻，将此时气压高度计输出的气压高度，记为初始气压高度HBaro_T0，将此时激光测距传感器输出激光测距高度，记为初始激光测距高度HLaser_T0，计算用于气压高度变化计算的初始高度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值越大，而激光测距传感器的健康参数实际上通过对传感器输出的激光测量距离值的噪声方差统计及超量程距离值个数统计共同确定。
进一步，步骤S5中对光流传感器输出的光流传感器数据，即像素坐标系u轴方向和v轴方向的像素位移，进行角运动补偿和物理尺度转换的过程具体如下：
本算法中，光流传感器采用底部安装，光流传感器的相机坐标系、IMU坐标系、磁力计坐标系与“右前上”载体坐标系保持安装一致，像素坐标系的u轴和载体坐标系的右向轴平行，像素坐标系的v轴与载体坐标系前向轴平行；所述光流传感器直接输出像素坐标系下的u轴像素位移OpFlowX和v轴像素位移OpFlowY，u轴像素位移OpFlowX和v轴像素位移OpFlowY包含线运动产生的像素位移和角运动产生的像素位移，为提取出线运动信息，需要补偿载体角运动产生的像素位移：
式(4)中，OpFlowX_transmotion、OpFlowY_transmotion为线运动产生的像素位移；γlast、θlast为上一有效光流传感器数据输出时的以弧度为单位的载体横滚角和俯仰角；K为光流传感器角运动补偿参数，K值为光流传感器手册中给出的一项传感器出厂参数，也可使光流传感器在固定高度下只做角运动测试得到。
得到(4)式中的线运动产生的像素位移后，将其转化为以米为单位的相机坐标系位移：
式(5)中OpFlowPx、OpFlowPy为两帧有效光流传感器数据之间的光流传感器在相机坐标系下右向和前向的位移，单位为米，因为，相机坐标系与载体坐标系重合，因此，该位移也是光流传感器在载体坐标系下的位移；resolution为光流传感器的分辨率，可以通过查光流传感器的手册获得，也可通过线运动下GPS速度标定、加速度计输出的加速度积分后的速度进行标定或者沿固定距离之间往复直线运动标定等方式得到该参数；PU为组合导航数据融合得到的高度。
得到(5)式中米为单位的光流传感器在载体坐标系下位移后，计算出米/秒为单位的光流传感器在载体系下的速度：
式(6)中，OpFlowVx、OpFlowVy为光流传感器在载体坐标系的右向和前向的速度；tnow为光流传感器当前帧输出时秒为单位的时间；tlast为光流传感器上一帧数据输出时秒为单位的时间。
进一步，步骤S6中，在进行导航数据融合时，涉及磁力计数据的处理，归一化的当地地理坐标系(东北天)下的地磁矢量[mE mN mU]T的计算方法为：
将载体坐标系下，磁力计原始输出的归一化值[mx my mz]T转换到地理坐标系：
其中，[mE1 mN1 mU1]T为通过姿态矩阵直接转换得到的地理坐标系下地磁矢量；为四元数构成的载体坐标系到导航坐标系的姿态矩阵。
利用式(7)中地理坐标系下地磁矢量[mE1 mN1 mU1]T，重构出地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T：
将式(8)中地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T，通过地磁偏角的补偿转换到地理坐标系下的地磁矢量[mE mN mU]T：
其中，Mag_dec为弧度为单位的地磁偏角，由经纬度查询得到。
基于站心坐标系下的，采用EKF融合光流传感器位置和速度的组合导航算法，组合导航算法状态方程为：
其中，为***状态，为***噪声；包括：3维位置3维速度4维姿态四元数3维陀螺零偏和3维加速度计零偏共16维；***噪声包括：3维陀螺仪白噪声3维加速度计白噪声共6维；为四元数的乘法矩阵；为IMU中加速度计输出加速度、为IMU中陀螺仪输出角速度；根据状态微分方程(10)求解状态一步预测。
组合导航算法量测方程：
其中，表示量测；为量测预测值；表示量测噪声；
式(11)中，量测噪声包括2维光流位移量测噪声、2维光流速度量测噪声、3维归一化后的磁力计输出量测噪声、1维融合高度量测噪声；
式(11)中，表示8维量测，可以用下式(12)表示：
式(12)中，表示2维光流传感器输出载体系右向和前向位移OpFlowPx与OpFlowPy、表示2维光流传感器输出载体系右向和前向速度OpFlowVx与OpFlowVy、表示3维由磁力计输出归一化后的磁力计量测、H表示1维气压高度和激光测距高度的融合高度。
式(11)中，为量测与状态非线性关系函数，可以表示为：
式(13)中和由式(14)和式(15)计算得到：
其中，为状态中东北天位置转化到载体系的右向和前向位移，即式(14)中载体系速度[Px Py Pz]T中的Px和Py；为状态中东北天速度转换到载体系的右向和前向速度，即式(15)中载体系速度[Vx Vy Vz]T中的Vx和Vy；PU即状态中的高度；为上一次光流数据有效时导航数据融合的东北天位置状态估计；为归一化的当地地理坐标系(东北天)下的地磁矢量；将状态一步预测值代入式(13)，即得到量测预测值。
式(11)中，量测噪声包括2维光流位移量测噪声、2维光流速度量测噪声、3维归一化后的磁力计输出量测噪声、1维融合高度量测噪声。
对式(10)和式(13)，通过计算雅克比矩阵，得到状态转移矩阵Φ、***噪声驱动矩阵Γ以及量测矩阵H；
状态转移矩阵Φ计算：
***噪声驱动矩阵Γ计算：
式(16)、(17)中T为导航周期，I为单位矩阵；
量测矩阵H计算：
状态一步预测值通过解微分方程获得：
式(19)中为上一导航周期状态估计值；
然后利用扩展卡尔曼滤波即可完成数据融合，解算出载体的位置、速度和姿态信息。
以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关的工作人员完全可以在不偏离本发明的范围内，进行多样的变更以及修改。本项发明的技术范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。
Claims (5)
1.一种融合光流位置和速度信息的组合导航算法，其特征在于：包括导航***，所述导航***包括光流传感器、IMU、磁力计、气压高度计和激光测距传感器，还包括以下步骤：
S1：由导航计算机读取安装于载体上的光流传感器、IMU、磁力计、气压高度计和激光测距传感器的数据信息，其中，从光流传感器读取的是像素坐标系下u轴方向和v轴方向的两帧有效图像之间的像素位移，从IMU上读取的是角速度和加速度数据，从磁力计上读取的是地磁强度数据，从气压高度计上读取的是气压高度数据，从激光测距传感器上读取的是激光测距高度数据；
S2：采用变权值法对步骤S1中获取的数据信息中的气压高度与激光测距高度进行融合，计算获得一个融合高度，并将融合高度作为扩展卡尔曼滤波数据融合的高度量测；
S3：导航算法根据光流传感器数据更新标志，判断光流传感器数据是否更新，如果光流传感器数据没有更新，进入步骤S4；如果光流传感器数据有更新，则进入步骤S5；
S4：在光流传感器数据无更新情况下，导航算法进行捷联纯惯性导航递推，解算出载体的位置、速度和姿态信息；
S5：在光流传感器数据有更新情况下，对光流传感器输出的两维像素位移进行角运动补偿，得到线运动对应的像素位移，并利用相机分辨率和相机与被拍摄平面之间的物理距离对线运动像素位移进行物理尺度转换，将线运动像素位移转化为米为单位的相机坐标系下右向和前向位移；并根据两帧有效光流传感器数据输出之间的间隔时间计算出相机坐标系下右向和前向速度；
S6：根据步骤S5的结果，通过扩展卡尔曼滤波对光流传感器数据计算得到的载体系位置和速度、IMU数据、磁力计数据、融合高度进行数据融合，解算出载体的位置、速度和姿态信息。
2.如权利要求1所述的融合光流位置和速度信息的组合导航算法，其特征在于：步骤S2中对气压高度与激光测距高度进行融合以计算融合高度的过程如下：
(1)确定初始高度HBaro0
在导航初始时刻，将此时气压高度计输出的气压高度，记为初始气压高度HBaro_T0，将此时激光测距传感器输出激光测距高度，记为初始激光测距高度HLaser_T0，计算用于气压高度变化计算的初始高度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值越大，而激光测距传感器的健康参数实际上通过对传感器输出的激光测量距离值的噪声方差统计及超量程距离值个数统计共同确定。
3.如权利要求2所述的融合光流位置和速度信息的组合导航算法，其特征在于：步骤S5中对光流传感器输出的光流传感器数据，即像素坐标系u轴方向和v轴方向的像素位移，进行角运动补偿和物理尺度转换的过程具体如下：
本算法中，光流传感器采用底部安装，光流传感器的相机坐标系、IMU坐标系、磁力计坐标系与“右前上”载体坐标系保持安装一致，像素坐标系的u轴和载体坐标系的右向轴平行，像素坐标系的v轴与载体坐标系前向轴平行；所述光流传感器直接输出像素坐标系下的u轴像素位移OpFlowX和v轴像素位移OpFlowY，u轴像素位移OpFlowX和v轴像素位移OpFlowY包含线运动产生的像素位移和角运动产生的像素位移，为提取出线运动信息，需要补偿载体角运动产生的像素位移：
式(4)中，OpFlowX_transmotion、OpFlowY_transmotion为线运动产生的像素位移；γlast、θlast为上一有效光流传感器数据输出时的以弧度为单位的载体横滚角和俯仰角；K为光流传感器角运动补偿参数；
得到(4)式中的线运动产生的像素位移后，将其转化为以米为单位的相机坐标系位移：
式(5)中OpFlowPx、OpFlowPy为两帧有效光流传感器数据之间的光流传感器在相机坐标系下右向和前向的位移，单位为米；resolution为光流传感器的分辨率；PU为组合导航数据融合得到的高度；
得到(5)式中米为单位的光流传感器在载体坐标系下位移后，计算出米/秒为单位的光流传感器在载体系下的速度：
式(6)中，OpFlowVx、OpFlowVy为光流传感器在载体坐标系的右向和前向的速度；tnow为光流传感器当前帧输出时秒为单位的时间；tlast为光流传感器上一帧数据输出时秒为单位的时间。
4.如权利要求3所述的姿态自主冗余组合导航算法，其特征在于：步骤S6中，在进行导航数据融合时，涉及磁力计数据的处理，归一化的当地地理坐标系下的地磁矢量[mE mN mU]T的计算方法为：
将载体坐标系下，磁力计原始输出的归一化值[mx my mz]T转换到地理坐标系：
其中，[mE1 mN1 mU1]T为通过姿态矩阵直接转换得到的地理坐标系下地磁矢量；为四元数构成的载体坐标系到导航坐标系的姿态矩阵；
利用式(7)中地理坐标系下地磁矢量[mE1 mN1 mU1]T，重构出地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T：
将式(8)中地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T，通过地磁偏角的补偿转换到地理坐标系下的地磁矢量[mE mN mU]T：
其中，Mag_dec为弧度为单位的地磁偏角，由经纬度查询得到。
5.如权利要求4所述的融合光流位置和速度信息的组合导航算法，其特征在于：基于站心坐标系下的，采用EKF融合光流传感器位置和速度的组合导航算法，组合导航算法状态方程为：
其中，为***状态，为***噪声；包括：3维位置3维速度4维姿态四元数3维陀螺零偏和3维加速度计零偏共16维；***噪声包括：3维陀螺仪白噪声3维加速度计白噪声共6维；为四元数的乘法矩阵；为IMU中加速度计输出加速度、为IMU中陀螺仪输出角速度；根据状态微分方程(10)求解状态一步预测；
组合导航算法量测方程：
其中，表示量测；为量测预测值；表示量测噪声；
式(11)中，量测噪声包括2维光流位移量测噪声、2维光流速度量测噪声、3维归一化后的磁力计输出量测噪声、1维融合高度量测噪声；
式(11)中，表示8维量测，可以用下式(12)表示：
式(12)中，表示2维光流传感器输出载体系右向和前向位移OpFlowPx与OpFlowPy、表示2维光流传感器输出载体系右向和前向速度OpFlowVx与OpFlowVy、表示3维由磁力计输出归一化后的磁力计量测、H表示1维气压高度和激光测距高度的融合高度；
式(11)中，为量测与状态非线性关系函数，可以表示为：
式(13)中和由式(14)和式(15)计算得到：
其中，为状态中东北天位置转化到载体系的右向和前向位移，即式(14)中载体系速度[Px Py Pz]T中的Px和Py；为状态中东北天速度转换到载体系的右向和前向速度，即式(15)中载体系速度[Vx Vy Vz]T中的Vx和Vy；PU即状态中的高度；为上一次光流数据有效时导航数据融合的东北天位置状态估计；为归一化的当地地理坐标系下的地磁矢量；将状态一步预测值代入式(13)，即得到量测预测值；
式(11)中，量测噪声包括2维光流位移量测噪声、2维光流速度量测噪声、3维归一化后的磁力计输出量测噪声、1维融合高度量测噪声；
对式(10)和式(13)，通过计算雅克比矩阵，得到状态转移矩阵Φ、***噪声驱动矩阵Γ以及量测矩阵H；
状态转移矩阵Φ计算：
***噪声驱动矩阵Γ计算：
式(16)、(17)中T为导航周期，I为单位矩阵；
量测矩阵H计算：
状态一步预测值通过解微分方程获得：
式(19)中为上一导航周期状态估计值；
然后利用扩展卡尔曼滤波即可完成数据融合，解算出载体的位置、速度和姿态信息。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910270669.5A CN109916394A (zh) | 2019-04-04 | 2019-04-04 | 一种融合光流位置和速度信息的组合导航算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910270669.5A CN109916394A (zh) | 2019-04-04 | 2019-04-04 | 一种融合光流位置和速度信息的组合导航算法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN109916394A true CN109916394A (zh) | 2019-06-21 |
Family
ID=66968658
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910270669.5A Pending CN109916394A (zh) | 2019-04-04 | 2019-04-04 | 一种融合光流位置和速度信息的组合导航算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109916394A (zh) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110428452A (zh) * | 2019-07-11 | 2019-11-08 | 北京达佳互联信息技术有限公司 | 非静态场景点的检测方法、装置、电子设备及存储介质 |
| CN110873813A (zh) * | 2019-12-02 | 2020-03-10 | 中国人民解放军战略支援部队信息工程大学 | 一种水流速度估算方法、组合导航方法及装置 |
| CN111445491A (zh) * | 2020-03-24 | 2020-07-24 | 山东智翼航空科技有限公司 | 微型无人机三邻域极大差值边缘检测狭道导引算法 |
| CN112254721A (zh) * | 2020-11-06 | 2021-01-22 | 南京大学 | 一种基于光流相机的姿态定位方法 |
| CN112284380A (zh) * | 2020-09-23 | 2021-01-29 | 深圳市富临通实业股份有限公司 | 一种基于光流和imu融合的非线性估计方法及*** |
| CN112923924A (zh) * | 2021-02-01 | 2021-06-08 | 杭州电子科技大学 | 一种锚泊船舶姿态与位置监测方法及*** |
| CN114018241A (zh) * | 2021-11-03 | 2022-02-08 | 广州昂宝电子有限公司 | 用于无人机的定位方法和设备 |
| CN114184194A (zh) * | 2021-11-30 | 2022-03-15 | 中国电子科技集团公司第二十九研究所 | 一种拒止环境下的无人机自主导航定位方法 |
| CN114216454A (zh) * | 2021-10-27 | 2022-03-22 | 湖北航天飞行器研究所 | 一种gps拒止环境下基于异源图像匹配的无人机自主导航定位方法 |
| CN118050814A (zh) * | 2024-04-16 | 2024-05-17 | 山东省地质矿产勘查开发局第五地质大队（山东省第五地质矿产勘查院） | 低空光学定姿三分量磁测***及测定磁场矢量数据的方法 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110206236A1 (en) * | 2010-02-19 | 2011-08-25 | Center Jr Julian L | Navigation method and aparatus |
| CN103344218A (zh) * | 2013-06-18 | 2013-10-09 | 桂林理工大学 | 一种适用于低空无人机的高度测量***及方法 |
| US20170212529A1 (en) * | 2013-11-27 | 2017-07-27 | 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) |
| CN106989744A (zh) * | 2017-02-24 | 2017-07-28 | 中山大学 | 一种融合机载多传感器的旋翼无人机自主定位方法 |
| CN107074360A (zh) * | 2016-11-22 | 2017-08-18 | 深圳市大疆创新科技有限公司 | 无人飞行器的控制方法、飞行控制器及无人飞行器 |
| CN109540126A (zh) * | 2018-12-03 | 2019-03-29 | 哈尔滨工业大学 | 一种基于光流法的惯性视觉组合导航方法 |
-
2019
- 2019-04-04 CN CN201910270669.5A patent/CN109916394A/zh active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110206236A1 (en) * | 2010-02-19 | 2011-08-25 | Center Jr Julian L | Navigation method and aparatus |
| CN103344218A (zh) * | 2013-06-18 | 2013-10-09 | 桂林理工大学 | 一种适用于低空无人机的高度测量***及方法 |
| US20170212529A1 (en) * | 2013-11-27 | 2017-07-27 | 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) |
| CN107074360A (zh) * | 2016-11-22 | 2017-08-18 | 深圳市大疆创新科技有限公司 | 无人飞行器的控制方法、飞行控制器及无人飞行器 |
| CN106989744A (zh) * | 2017-02-24 | 2017-07-28 | 中山大学 | 一种融合机载多传感器的旋翼无人机自主定位方法 |
| CN109540126A (zh) * | 2018-12-03 | 2019-03-29 | 哈尔滨工业大学 | 一种基于光流法的惯性视觉组合导航方法 |
Non-Patent Citations (1)
| Title |
|---|
| 杨天雨等: "惯性/光流/磁组合导航技术在四旋翼飞行器中的应用", 《传感器与微***》 * |
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110428452B (zh) * | 2019-07-11 | 2022-03-25 | 北京达佳互联信息技术有限公司 | 非静态场景点的检测方法、装置、电子设备及存储介质 |
| CN110428452A (zh) * | 2019-07-11 | 2019-11-08 | 北京达佳互联信息技术有限公司 | 非静态场景点的检测方法、装置、电子设备及存储介质 |
| CN110873813A (zh) * | 2019-12-02 | 2020-03-10 | 中国人民解放军战略支援部队信息工程大学 | 一种水流速度估算方法、组合导航方法及装置 |
| CN111445491A (zh) * | 2020-03-24 | 2020-07-24 | 山东智翼航空科技有限公司 | 微型无人机三邻域极大差值边缘检测狭道导引算法 |
| CN111445491B (zh) * | 2020-03-24 | 2023-09-15 | 山东智翼航空科技有限公司 | 微型无人机三邻域极大差值边缘检测狭道导引方法 |
| CN112284380A (zh) * | 2020-09-23 | 2021-01-29 | 深圳市富临通实业股份有限公司 | 一种基于光流和imu融合的非线性估计方法及*** |
| CN112254721A (zh) * | 2020-11-06 | 2021-01-22 | 南京大学 | 一种基于光流相机的姿态定位方法 |
| CN112923924A (zh) * | 2021-02-01 | 2021-06-08 | 杭州电子科技大学 | 一种锚泊船舶姿态与位置监测方法及*** |
| CN114216454A (zh) * | 2021-10-27 | 2022-03-22 | 湖北航天飞行器研究所 | 一种gps拒止环境下基于异源图像匹配的无人机自主导航定位方法 |
| CN114216454B (zh) * | 2021-10-27 | 2023-09-08 | 湖北航天飞行器研究所 | 一种gps拒止环境下基于异源图像匹配的无人机自主导航定位方法 |
| CN114018241A (zh) * | 2021-11-03 | 2022-02-08 | 广州昂宝电子有限公司 | 用于无人机的定位方法和设备 |
| CN114018241B (zh) * | 2021-11-03 | 2023-12-26 | 广州昂宝电子有限公司 | 用于无人机的定位方法和设备 |
| CN114184194A (zh) * | 2021-11-30 | 2022-03-15 | 中国电子科技集团公司第二十九研究所 | 一种拒止环境下的无人机自主导航定位方法 |
| CN118050814A (zh) * | 2024-04-16 | 2024-05-17 | 山东省地质矿产勘查开发局第五地质大队（山东省第五地质矿产勘查院） | 低空光学定姿三分量磁测***及测定磁场矢量数据的方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109916394A (zh) | 一种融合光流位置和速度信息的组合导航算法 | |
| CN104736963B (zh) | 测绘***和方法 | |
| CN109931926B (zh) | 一种基于站心坐标系的小型无人机无缝自主式导航方法 | |
| CN111678538B (zh) | 一种基于速度匹配的动态水平仪误差补偿方法 | |
| CN109540126A (zh) | 一种基于光流法的惯性视觉组合导航方法 | |
| Ladetto et al. | Digital magnetic compass and gyroscope integration for pedestrian navigation | |
| CN109186597B (zh) | 一种基于双mems-imu的室内轮式机器人的定位方法 | |
| CN109269471A (zh) | 一种新型gnss接收机倾斜测量***及方法 | |
| CN107270893A (zh) | 面向不动产测量的杆臂、时间不同步误差估计与补偿方法 | |
| CN101290229A (zh) | 硅微航姿***惯性/地磁组合方法 | |
| CN107490378B (zh) | 一种基于mpu6050与智能手机的室内定位与导航的方法 | |
| CN111024070A (zh) | 一种基于航向自观测的惯性足绑式行人定位方法 | |
| CN104181573B (zh) | 北斗惯导深组合导航微*** | |
| CN201955092U (zh) | 一种基于地磁辅助的平台式惯性导航装置 | |
| CN107270898B (zh) | 基于mems传感器和vlc定位融合的双粒子滤波导航装置和方法 | |
| Hartmann et al. | Indoor 3D position estimation using low-cost inertial sensors and marker-based video-tracking | |
| CN103512584A (zh) | 导航姿态信息输出方法、装置及捷联航姿参考*** | |
| CN109916395A (zh) | 一种姿态自主冗余组合导航算法 | |
| Wahdan et al. | Three-dimensional magnetometer calibration with small space coverage for pedestrians | |
| CN107014377A (zh) | 一种基于惯性定位的多功能鞋垫 | |
| CN110044378A (zh) | 一种用于水下深潜器的光纤捷联惯性导航高精度定位***及方法 | |
| CN109540135A (zh) | 水田拖拉机位姿检测和偏航角提取的方法及装置 | |
| CN107015259A (zh) | 采用多普勒测速仪计算伪距/伪距率的紧组合方法 | |
| CN108458714A (zh) | 一种姿态检测***中不含重力加速度的欧拉角求解方法 | |
| CN109579836A (zh) | 一种基于mems惯性导航的室内行人方位校准方法 |
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 | ||
| AD01 | Patent right deemed abandoned |
Effective date of abandoning: 20230609 |
|
| AD01 | Patent right deemed abandoned |