CN114396943A - 一种融合定位方法与终端 - Google Patents

一种融合定位方法与终端 Download PDF

Info

Publication number
CN114396943A
CN114396943A CN202210039694.4A CN202210039694A CN114396943A CN 114396943 A CN114396943 A CN 114396943A CN 202210039694 A CN202210039694 A CN 202210039694A CN 114396943 A CN114396943 A CN 114396943A
Authority
CN
China
Prior art keywords
positioning data
error
equation
information
positioning
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
Application number
CN202210039694.4A
Other languages
English (en)
Other versions
CN114396943B (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.)
State Grid Siji Shenwang Position Service Beijing Co ltd
State Grid Corp of China SGCC
State Grid Information and Telecommunication Co Ltd
Original Assignee
State Grid Siji Shenwang Position Service Beijing Co ltd
State Grid Corp of China SGCC
State Grid Information and Telecommunication 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 State Grid Siji Shenwang Position Service Beijing Co ltd, State Grid Corp of China SGCC, State Grid Information and Telecommunication Co Ltd filed Critical State Grid Siji Shenwang Position Service Beijing Co ltd
Priority to CN202210039694.4A priority Critical patent/CN114396943B/zh
Publication of CN114396943A publication Critical patent/CN114396943A/zh
Application granted granted Critical
Publication of CN114396943B publication Critical patent/CN114396943B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种融合定位方法与终端；接收单目摄像头的环境信息，根据环境信息估计单目摄像头的位置和姿态；接收惯性测量单元的第一定位数据，根据单目摄像头的位置和姿态对第一定位数据进行优化；接收惯性导航***的第二定位数据，根据第二定位数据辅助全球导航卫星***进行多频多模的实时差分定位，得到第三定位数据；根据第一定位数据以及第三定位数据建立量测方程和状态方程；根据第一定位数据、量测方程和状态方程，采用扩展卡尔曼滤波算法估计状态量误差，并基于状态量误差对第一定位数据进行补偿，得到最终定位数据；综合全球导航卫星***的长时绝对位置与IMU和单目视觉短时精确推算位置的优点，提高了定位精度。

Description

一种融合定位方法与终端
技术领域
本发明涉及定位技术领域，特别涉及一种融合定位方法与终端。
背景技术
受限于作业现场复杂的环境，GNSS定位连续性和可信度带来了很大的挑战，受到遮挡和电磁干扰，卫星信号经常中断，由于频繁的重新初始化，浮点模糊度的精度受到限制，产生较大的定位偏差。为了提高在复杂环境下的定位性能，集成卫星导航和惯性导航***被广泛采用，用以提供连续定位、速度测量、姿态测量。然而MEMS-IMU的主要缺点是在缺乏有效控制的情况下，其导航误差会在短时间内迅速发散，导致定位不够精确。
发明内容
本发明所要解决的技术问题是：提供一种融合定位方法与终端，提高定位精度。
为了解决上述技术问题，本发明采用的技术方案为：
一种融合定位方法，包括步骤：
S1、向单目摄像头获取环境信息，根据所述环境信息估计所述单目摄像头的位置和姿态；
S2、向惯性测量单元获取第一定位数据，根据所述单目摄像头的位置和姿态对所述第一定位数据进行优化；
S3、向惯性导航***获取第二定位数据，根据所述第二定位数据辅助全球导航卫星***进行多频多模的实时差分定位，得到第三定位数据；
S4、根据所述第一定位数据以及所述第三定位数据建立量测方程和状态方程；
S5、根据所述第一定位数据、所述量测方程和所述状态方程，采用扩展卡尔曼滤波算法估计状态量误差，并基于所述状态量误差对所述第一定位数据进行补偿，得到最终定位数据。
为了解决上述技术问题，本发明采用的另一种技术方案为：
一种融合定位终端，包括处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以上方案中的步骤。
本发明的有益效果在于：本发明的技术方案将多频多模实时差分定位、惯性测量单元和单目摄像头所获取的数据采用扩展卡尔曼算法进行融合，综合全球导航卫星***的长时绝对位置与IMU和单目视觉短时精确推算位置的优点，以获得最佳位置状态的估计，采用IMU恢复单目视觉的度量尺度，提高运动跟踪性能，用视觉测量限制IMU的误差漂移，获得更高精度且可信的位置、姿态，从而提高了定位精度。
附图说明
图1为本发明实施例的一种融合定位方法的流程图；
图2为本发明实施例的一种融合定位终端的结构图；
图3为本发明实施例的一种融合定位方法的数据处理示意图；
标号说明：
1、一种融合定位终端；2、处理器；3、存储器。
具体实施方式
为详细说明本发明的技术内容、所实现目的及效果，以下结合实施方式并配合附图予以说明。
请参照图1以及图2，一种融合定位方法，包括步骤：
S1、向单目摄像头获取环境信息，根据所述环境信息估计所述单目摄像头的位置和姿态；
S2、向惯性测量单元获取第一定位数据，根据所述单目摄像头的位置和姿态对所述第一定位数据进行优化；
S3、向惯性导航***获取第二定位数据，根据所述第二定位数据辅助全球导航卫星***进行多频多模的实时差分定位，得到第三定位数据；
S4、根据所述第一定位数据以及所述第三定位数据建立量测方程和状态方程；
S5、根据所述第一定位数据、所述量测方程和所述状态方程，采用扩展卡尔曼滤波算法估计状态量误差，并基于所述状态量误差对所述第一定位数据进行补偿，得到最终定位数据。
从上述描述可知，本发明的有益效果在于：本发明的技术方案将多频多模实时差分定位、惯性测量单元和单目摄像头所获取的数据采用扩展卡尔曼算法进行融合，综合全球导航卫星***的长时绝对位置与IMU和单目视觉短时精确推算位置的优点，以获得最佳位置状态的估计，采用IMU恢复单目视觉的度量尺度，提高运动跟踪性能，用视觉测量限制IMU的误差漂移，获得更高精度且可信的位置、姿态，从而提高了定位精度。
进一步地，所述步骤S1具体包括步骤：
S11、接收单目摄像头的环境信息，对所述环境信息进行预处理；
S12、在所述环境信息的一段连续图像中提取Harris角点特征和SIFT尺度不变特征的特征点，并采用LK稀疏光流算法跟踪相邻图像中的所述特征点；
S13、根据所述相邻图像中的所述特征点，估计路标点以及所述单目摄像头的相对运动，并结合所述路标点和所述单目摄像头的相对运动估计所述单目摄像头的位置和姿态。
由上述描述可知，在得到单目摄像头采集的环境信息后，通过对其中相邻图像帧中的特征点进行跟踪，能够准确地得到单目摄像头的位置和姿态。
进一步地，所述步骤S12和步骤S13之间还包括步骤：
S121、通过仿射检验方法检验特征点的正确性并判断特征点数量是否充足，将正确性验证错误的特征点进行去除，并在特征点数量不足时，添加新的特征点。
由上述描述可知，本发明在提取特征点之后，通过仿射检验方法检验特征点的正确性，并去除错误的特征点，从而使后续所确定的单目摄像头的位置和姿态更加准确。
进一步地，所述第一定位数据包括第一位置信息、第一速度信息和第一姿态信息；
所述步骤S2具体包括步骤：
S21、接收惯性测量单元的三轴的角速度、加速度和静止时的重力向量，并对预设时间内的所述角速度、所述加速度和所述重力向量进行积分，得到所述第一位置信息和所述第一速度信息，并根据四元数的更新得到所述第一姿态信息；
S22、根据所述第一位置信息、所述第一速度信息、所述第一姿态信息以及所述预设时间内所述单目摄像头估计的位置和姿态，建立位置误差方程和姿态误差方程；
S23、对所述位置误差方程和所述姿态误差方程进行优化求解，并根据计算结果对所述第一位置信息和所述第一姿态信息进行优化补偿。
由上述描述可知，根据单目摄像头估计的位置和姿态信息，对由惯性测量单元采集数据计算的第一位置信息和第一姿态信息进行优化补偿，即利用视觉测量限制了惯性测量单元的误差漂移，以得到更高精度且可信的位置和姿态数据。
进一步地，所述全球导航卫星***进行实时差分定位的解算时还包括步骤：根据采集的伪距、载波和多普勒观测值，得到接收机和卫星间的第一伪距双差模型和第一相位双差模型，并进行整周模糊度解；
所述步骤S3具体包括步骤：
S31、接收惯性导航***的第二定位数据，判断所述整周模糊度解中所述全球导航卫星***采集的所述伪距、所述载波和所述多普勒观测值是否存在因信号中断产生的缺失，若存在缺失则将所述第一伪距双差模型和所述第一相位双差模型在中断处根据所述第二定位数据进行线性化处理，并结合历史模糊度和协方差采用最小二乘法估计最优浮点模糊度解，采用LAMBDA方法计算得到整周模糊度解；
S32、根据所述整周模糊度解继续多频多模的实时差分定位的解算，最终得到所述第三定位数据。
由上述描述可知，本方案在全球导航卫星***的微型信号存在中断丢失时，根据惯性导航***的定位数据进行线性处理，并通过最小二乘法以及LAMBDA方法计算整周模糊度解，弥补了全球导航卫星***的易受环境影响信号失锁的缺陷，提高了定位准确性。
进一步地，所述步骤S31和S32之间还包括步骤：
S311、对所述整周模糊度解进行正确性校验，根据全球导航卫星***信号正常时保存的历史整周模糊度解，进行假设检验统计量的统计分析，计算得到预设置信水平下的模糊度阈值；
S312、判断所述整周模糊度解是否落入所述模糊度阈值的范围内，若是则校验正确，否则校验错误，采用伪距测量的方法，计算整周模糊度解。
由上述描述可知，本发明还采用假设检验的方法确定模糊度解是否可取，以提高模糊度解决方案的可靠性。
进一步地，所述第一定位数据包括第一位置信息、第一速度信息和第一姿态信息；
所述步骤S4具体包括步骤：
S41、根据所述第一位置信息构建第二伪距双差模型，并根据所述第一伪距双差模型和所述第二伪距双差模型分别计算得到第一双差伪距、第二双差伪距、第一双差伪距率和第二双差伪距率，根据所述第一双差伪距和所述第二双差伪距间的误差以及所述第一双差伪距率和所述第二双差伪距率间的误差作为数据融合的观测量，构造量测方程；
S42、根据所述第一位置信息、所述第一速度信息和所述第一姿态信息建立状态方程，所述状态方程包括位置方程、速度方程以及姿态方程。
进一步地，所述状态量误差包括速度误差、姿态误差以及位置误差；
所述步骤S5具体包括步骤：
S51、根据所述第一位置信息、所述第一速度信息、所述第一姿态信息、所述量测方程、位置方程、速度方程以及姿态方程，采用扩展卡尔曼滤波算法估计所述速度误差、所述姿态误差以及所述位置误差；
S52、根据所述位置误差、所述速度误差以及所述姿态误差分别对所述第一位置信息、所述第一速度信息以及所述第一姿态信息进行补偿优化，得到最终定位数据。
由上述描述可知，本申请的技术方案基于单目视觉优化后的惯性测量单元的定位数据、全球导航卫星***的定位数据和惯性测量单元的量测方程和状态方程，采用扩展卡尔曼滤波算法对状态量各误差进行估计，对惯性测量单元得到的位置、速度、姿态信息进行补偿，从而得到较高精度的信息，提高了定位的精确度。
进一步地，所述步骤S5之后还包括步骤：
S6、将所述最终定位数据更新至所述惯性测量单元的所述第一定位数据。
由上述描述可知，将最终得到的较高精度的位置、速度、姿态信息向惯性测量单元推算的初始值进行更新，减少惯性测量单元的漂移影响。
请参照图2，一种融合定位终端，包括处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以上方案的步骤。
本发明的一种融合定位方法与终端，适用于需要进行定位，但因作业现场较为复杂，全球导航卫星***常因受到遮挡、电磁干扰发生卫星信号中断，导致定位数据不够准确的场景。
请参照图1和图3，本发明的实施例一为：
一种融合定位方法，包括步骤：
S1、向单目摄像头获取环境信息，根据所述环境信息估计所述单目摄像头的位置和姿态；
所述步骤S1具体包括步骤：
S11、接收单目摄像头的环境信息，对所述环境信息进行预处理；
S12、在所述环境信息的一段连续图像中提取Harris角点特征和SIFT尺度不变特征的特征点，并采用LK稀疏光流算法跟踪相邻图像中的所述特征点；
所述步骤S12和步骤S13之间还包括步骤：
S 121、通过仿射检验方法检验特征点的正确性并判断特征点数量是否充足，将正确性验证错误的特征点进行去除，并在特征点数量不足时，添加新的特征点；
S13、根据所述相邻图像中的所述特征点，估计路标点以及所述单目摄像头的相对运动，并结合所述路标点和所述单目摄像头的相对运动估计所述单目摄像头的位置和姿态。
本实施例中，采用单目摄像头识别环境地图，读取单目摄像头采集的信息并进行预处理，包括滤波、矫正和灰度处理。在一段连续的相邻帧图像中提取Harris角点特征和SIFT尺度不变特征，采用LK稀疏光流算法跟踪相邻帧特征，同时采用仿射检验方法检验特征点的正确性，去除跟踪错误的特征点，当特征点数量不足时，添加新的特征点。继而根据相邻图像中特征点估计路标点以及所述单目摄像头的相对运动，并结合所述路标点和所述单目摄像头的相对运动估计所述单目摄像头的位置和姿态，得到视觉里程计。视觉里程计包括位置、姿态、路标点坐标和相对运动。
S2、向惯性测量单元获取第一定位数据，根据所述单目摄像头的位置和姿态对所述第一定位数据进行优化；
所述第一定位数据包括第一位置信息、第一速度信息和第一姿态信息；
所述步骤S2具体包括步骤：
S21、接收惯性测量单元的三轴的角速度、加速度和静止时的重力向量，并对预设时间内的所述角速度、所述加速度和所述重力向量进行积分，得到所述第一位置信息和所述第一速度信息，并根据四元数的更新得到所述第一姿态信息；
S22、根据所述第一位置信息、所述第一速度信息、所述第一姿态信息以及所述预设时间内所述单目摄像头估计的位置和姿态，建立位置误差方程和姿态误差方程；
S23、对所述位置误差方程和所述姿态误差方程进行优化求解，并根据计算结果对所述第一位置信息和所述第一姿态信息进行优化补偿。
本实施例中，MEMS-IMU(惯性测量单元，IMU)可测量得到三轴的角速度、加速度和静止时的重力向量，在短时间内对其积分可得到位置、速度信息，根据四元数的更新可获得其姿态的信息。在连续帧图像间，采用IMU数据获得相对于惯性空间的位置、速度和旋转，结合单目摄像头估计的位置和姿态建立误差方程，对误差方程进行优化求解来补偿IMU的积分项，从而得到优化后的第一位置信息和第一姿态信息。
其中，四元数更新方程为：
Figure BDA0003466937400000081
Figure BDA0003466937400000082
为陀螺仪输出角速度右乘矩阵形式，
Figure BDA0003466937400000083
表示t时刻世界坐标系(其上标W)下的IMU旋转四元数，其下标B表示IMU坐标系；
根据单目摄像头采集的数据和IMU采集的数据估计对应两相邻图像间的位置、速度和旋转。
Figure BDA0003466937400000084
其中，
Figure BDA0003466937400000085
表示世界坐标系k帧图像IMU的位置；
Figure BDA0003466937400000086
表示世界坐标系k帧图像IMU的速度；Δtk为k帧图像的时间步长；
Figure BDA0003466937400000087
表示IMU测量的加速度；
Figure BDA0003466937400000088
表示加速度偏差；gw表示重力加速度；
Figure BDA0003466937400000089
表示IMU测量的角速度；
Figure BDA00034669374000000810
表示角速度偏差；
Figure BDA00034669374000000811
表示k帧图像下IMU的四元数。
即对相邻两图像间对应的IMU采集的数据进行预积分：
Figure BDA00034669374000000812
其中，
Figure BDA00034669374000000813
表示k帧图像到k+1帧图像间推算的相对位移、相对速度，
Figure BDA00034669374000000814
和
Figure BDA00034669374000000815
为预积分项，反应两图像帧间的相对位移、速度、旋转：
Figure BDA0003466937400000091
Figure BDA0003466937400000092
Figure BDA0003466937400000093
其中，
Figure BDA0003466937400000094
表示四元数乘法运算的左乘矩阵形式。
根据预积分项建立方程：
Figure BDA0003466937400000095
其中，I3×1表示3乘1阶单位阵；
Figure BDA0003466937400000096
表示摄像头系到IMU系的旋转矩阵；
Figure BDA0003466937400000097
表示摄像头k+1帧图像下计算的位移。
表示为：
Figure BDA00034669374000000911
在多帧图像间可表示为求解最小二乘方程：
Figure BDA0003466937400000098
其中，argmin表示取最小值；Γ表示滑动窗口中所有的图像帧。
通过这种方式可求得每一帧图像的运动速度
Figure BDA0003466937400000099
重力向量为
Figure BDA00034669374000000910
以及视觉尺度因子S。根据重力向量与传感器各轴的夹角可计算出姿态角。
IMU的推算部分如下：
xk+1＝Axk
xk为K时刻的位置速度加速度：
xk＝[L B H vE vN vU ae an au]；
其中，下标E、e表示ENU坐标系E方向；L B H vE vN vU ae an au依次分别为经度、纬度、高、E向速度、N向速度、U向速度、E向加速度、N向加速度、U向加速度。
A的值为：
Figure BDA0003466937400000101
其中，RM为卯酉圈曲率半径，RN为子午圈曲率半径，Δt为上一时刻与当前时刻的间隔。
姿态的计算采用四元数更新的方法，然后根据四元数计算出姿态角：
Figure BDA0003466937400000102
其中，γ、θ、ψ分别为俯仰、横滚和航向的姿态角，四元数q＝(q0，q1，q2，q3)。
S3、向惯性导航***获取第二定位数据，根据所述第二定位数据辅助全球导航卫星***进行多频多模的实时差分定位，得到第三定位数据；
所述全球导航卫星***进行实时差分定位的解算时还包括步骤：根据采集的伪距、载波和多普勒观测值，得到接收机和卫星间的第一伪距双差模型和第一相位双差模型，并进行整周模糊度解；
所述步骤S3具体包括步骤：
S31、接收惯性导航***的第二定位数据，判断所述整周模糊度解中所述全球导航卫星***采集的所述伪距、所述载波和所述多普勒观测值是否存在因信号中断产生的缺失，若存在缺失则将所述第一伪距双差模型和所述第一相位双差模型在中断处根据所述第二定位数据进行线性化处理，并结合历史模糊度和协方差采用最小二乘法估计最优浮点模糊度解，采用LAMBDA方法计算得到整周模糊度解；
所述步骤S31和S32之间还包括步骤：
S311、对所述整周模糊度解进行正确性校验，根据全球导航卫星***信号正常时保存的历史整周模糊度解，进行假设检验统计量的统计分析，计算得到预设置信水平下的模糊度阈值；
S312、判断所述整周模糊度解是否落入所述模糊度阈值的范围内，若是则校验正确，否则校验错误，采用伪距测量的方法，计算整周模糊度解。
S32、根据所述整周模糊度解继续多频多模的实时差分定位的解算，最终得到所述第三定位数据。
本实施例中，在GNSS(全球导航卫星***)解算中，由伪距、载波、多普勒观测值作为原始数据，得到接收机和卫星间的伪距和相位双差模型。在进行整周模糊度解中，基于GNSS信号易受到外界干扰，而INS(惯性导航***)不易受到外界干扰的特点，使用INS输出的位置信息辅助GNSS固定整周模糊度，在GNSS信息中断时，将伪距和相位双差模型在INS推算的位置处进行线性化处理，结合历史模糊度和协方差采用最小二乘方法估计最优浮点模糊度解，然后采用LAMBDA方法计算整周模糊度向量，并进行正确性验证。
具体的，在出现GNSS信号中断，***初始化后，INS提供高速率的导航信息输出，包括位置、速度、姿态(PVA)。由于多频GNSS的基准站固定不变且已知地理位置，在基准站和流动站共同接收多频GNSS信号、流动站接收基准站信息后，可采用RTK(实时差分定位)的定位方法，确定流动站的位置。根据RTK原理，在流动站接收到的基准值载波相位观测值可用前提下，形成双差伪距和载波相位观测。使用最小二乘平差计算双差浮点模糊解和它们相应的协方差最优解，得出位置约束。然后采用LAMBDA方法进行模糊度解算，并进行假设检验以验证模糊度解的正确性，即正确性校验，在GNSS信号正常时保存LAMBDA解算的模糊度，然后根据一般假设检验统计量的统计分析，计算得到一定置信水平条件下的合理阈值，根据与阈值的比较，以确定是否应接受或不接受搜索到的模糊解。如果通过正确性校验，则将当前数据用于后续步骤，否则，使用伪距测量。
S4、根据所述第一定位数据以及所述第三定位数据建立量测方程和状态方程；
所述步骤S4具体包括步骤：
S41、根据所述第一位置信息构建第二伪距双差模型，并根据所述第一伪距双差模型和所述第二伪距双差模型分别计算得到第一双差伪距、第二双差伪距、第一双差伪距率和第二双差伪距率，根据所述第一双差伪距和所述第二双差伪距间的误差以及所述第一双差伪距率和所述第二双差伪距率间的误差作为数据融合的观测量，构造量测方程；
S42、根据所述第一位置信息、所述第一速度信息和所述第一姿态信息建立状态方程，所述状态方程包括位置方程、速度方程以及姿态方程。
本实施例中，由GNSS与IMU进行紧组合，建立量测方程和状态方程。基于IMU推算的位置可构造惯导的伪距双差模型，GNSS和IMU数据分别计算得到双差伪距和双差伪距率，两者间的误差作为数据融合的观测量，构造量测方程。
S5、根据所述第一定位数据、所述量测方程和所述状态方程，采用扩展卡尔曼滤波算法估计状态量误差，并基于所述状态量误差对所述第一定位数据进行补偿，得到最终定位数据。
所述状态量误差包括速度误差、姿态误差以及位置误差；
所述步骤S5具体包括步骤：
S51、根据所述第一位置信息、所述第一速度信息、所述第一姿态信息、所述量测方程、位置方程、速度方程以及姿态方程，采用扩展卡尔曼滤波算法估计所述速度误差、所述姿态误差以及所述位置误差；
S52、根据所述位置误差、所述速度误差以及所述姿态误差分别对所述第一位置信息、所述第一速度信息以及所述第一姿态信息进行补偿优化，得到最终定位数据。
所述步骤S5之后还包括步骤：
S6、将所述最终定位数据更新至所述惯性测量单元的所述第一定位数据。
本实施例中，状态量误差为姿态误差、速度误差、位置误差、陀螺仪误差、加速度计误差、钟差等效距离误差以及钟漂等效伪距率误差。基于单目视觉优化后的IMU数据、GNSS和IMU的量测方程和状态方程，采用扩展卡尔曼滤波算法对状态量各误差进行估计，最终对位置、速度、姿态进行补偿，得到较高精度的信息。同时将获得的高精度位置、速度、姿态对IMU推算初始值进行更新，减小IMU的漂移影响。
其中，状态方程包括IMU误差状态方程和GNSS误差状态方程，以由姿态误差、速度误差、位置误差、陀螺仪和加速度计误差构成的状态方程为例，如下所示：
Figure BDA0003466937400000131
其中，WI为随机噪声，XI为状态：
Figure BDA0003466937400000132
其中，
Figure BDA0003466937400000133
依次分别表示经度误差、纬度误差、高误差、E向速度误差、N向速度误差、U向速度误差、E轴失准角误差、N轴失准角误差、U轴失准角误差、x轴加速度偏差、y轴加速度偏差、z轴加速度偏差、x轴角速度偏差、y轴角速度偏差、z轴角速度偏差。
GI为噪声转换矩阵：
Figure BDA0003466937400000134
其中，zero(3，3)表示3X3阶0矩阵，
Figure BDA0003466937400000135
表示载体坐标系到世界坐标系的旋转矩阵；
FI为状态转移矩阵：
Figure BDA0003466937400000141
Figure BDA0003466937400000142
Figure BDA0003466937400000143
Figure BDA0003466937400000144
Figure BDA0003466937400000145
Figure BDA0003466937400000146
Figure BDA0003466937400000147
Figure BDA0003466937400000151
Figure BDA0003466937400000152
其中，RM为卯酉圈曲率半径，RN为子午圈曲率半径，wie为ECEF系下地球旋转角速度，fe、fn、fu分别为e、n、u方向的加速度与其误差之和。
钟差等效距离误差、钟漂等效伪距率误差方程如下所示：
Figure BDA0003466937400000153
其中：
XG＝[δtu δtru]T
WG＝[wtu wtru]T
Figure BDA0003466937400000154
Figure BDA0003466937400000155
其中tu为钟差等效距离，tru为钟漂等效伪距率，βtru为误差相关时间，wtu、wtru为随机白噪声。
量测方程，即观测方程包括IMU误差观测方程和伪距伪距率误差观测方程，其中，IMU观测方程为：
ZI＝HIXI+VI
其中，ZI为观测向量，HI为观测矩阵，VI为观测误差：
Figure BDA0003466937400000161
Figure BDA0003466937400000162
其中，eye(3，3)表示3X3单位阵，zero(3，3)表示3X3阶0矩阵。
伪距、伪距率误差观测方程为：
ZG＝HGXG+UG+VG
其中，
Figure BDA0003466937400000163
Figure BDA0003466937400000164
Figure BDA0003466937400000165
其中，δx为IMU推算的运动载***置与卫星星历得到的卫星位置之差，
Figure BDA0003466937400000166
为IMU推算的运动载体速度与卫星星历得到的卫星速度之差，VG为伪距、伪距率观测误差。
结合以上状态方程和观测方程，采用扩展卡尔曼进行估计具体为：
(1)对***模型线性化处理：
Figure BDA0003466937400000167
其中，fk-1表示非线性函数；ωk-1表示过程噪声；xk为k时刻真值，
Figure BDA0003466937400000168
为k-1时刻状态估计值，
Figure BDA0003466937400000169
为k-1时刻预测误差值。
一阶泰勒展开：
Figure BDA00034669374000001610
其中，
Figure BDA0003466937400000171
表示非线性函数对状态x的偏导，
Figure BDA0003466937400000172
表示非线性函数对误差w的偏导。
(2)通过k-1时刻的状态值对时刻k进行一步预测：
Figure BDA0003466937400000173
一步状态预测所产生的误差为：
Figure BDA0003466937400000174
状态预测误差的协方差阵是：
Figure BDA0003466937400000175
其中，Pk-1|k-1表示k-1时刻的状态协方差阵，Qk-1表示k-1时刻的误差协方差，上标T表示转置矩阵。
(3)k时刻量测的线性化方程为：
Figure BDA0003466937400000176
其中，zk表示观测量，hk表示观测函数，vk表示观测噪声，
Figure BDA0003466937400000177
表示观测函数对状态x的偏导，
Figure BDA0003466937400000178
表示观测函数对噪声v的偏导。
(4)k时刻量测的一步预测：
Figure BDA0003466937400000179
量测预测误差为：
Figure BDA00034669374000001710
状态预测误差与量测预测误差协方差阵为：
Figure BDA00034669374000001711
(5)状态滤波的更新公式为：
状态更新公式：
Figure BDA0003466937400000181
协方差更新公式：
Figure BDA0003466937400000182
其中，I表示单位阵。
k时刻卡尔曼增益K为：
Figure BDA0003466937400000183
请参照图2，本发明的实施例二为：
一种融合定位终端1，包括处理器2、存储器3以及存储在所述存储器3中并可在所述处理器2上运行的计算机程序，所述处理器2执行所述计算机程序时实现以上实施例一中的步骤。
本发明的主要原理在于，将多频多模实时差分定位、惯性测量单元和单目摄像头所获取的数据采用扩展卡尔曼算法进行融合，来提高定位精度。
综上所述，本发明提供的一种融合定位方法与终端，将多频多模实时差分定位、惯性测量单元和单目摄像头所获取的数据采用扩展卡尔曼算法进行融合，综合全球导航卫星***的长时绝对位置与IMU和单目视觉短时精确推算位置的优点，以获得最佳位置状态的估计，采用IMU恢复单目视觉的度量尺度，提高运动跟踪性能，用视觉测量限制IMU的误差漂移，获得更高精度且可信的位置、姿态，从而提高了定位精度。
以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等同变换，或直接或间接运用在相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种融合定位方法，其特征在于，包括步骤：
S1、接收单目摄像头的环境信息，根据所述环境信息估计所述单目摄像头的位置和姿态；
S2、接收惯性测量单元的第一定位数据，根据所述单目摄像头的位置和姿态对所述第一定位数据进行优化；
S3、接收惯性导航***的第二定位数据，根据所述第二定位数据辅助全球导航卫星***进行多频多模的实时差分定位，得到第三定位数据；
S4、根据所述第一定位数据以及所述第三定位数据建立量测方程和状态方程；
S5、根据所述第一定位数据、所述量测方程和所述状态方程，采用扩展卡尔曼滤波算法估计状态量误差，并基于所述状态量误差对所述第一定位数据进行补偿，得到最终定位数据。
2.根据权利要求1所述的一种融合定位方法，其特征在于，所述步骤S1具体包括步骤：
S11、接收单目摄像头的环境信息，对所述环境信息进行预处理；
S12、在所述环境信息的一段连续图像中提取Harris角点特征和SIFT尺度不变特征的特征点，并采用LK稀疏光流算法跟踪相邻图像中的所述特征点；
S13、根据所述相邻图像中的所述特征点，估计路标点以及所述单目摄像头的相对运动，并结合所述路标点和所述单目摄像头的相对运动估计所述单目摄像头的位置和姿态。
3.根据权利要求2所述的一种融合定位方法，其特征在于，所述步骤S12和步骤S13之间还包括步骤：
S121、通过仿射检验方法检验特征点的正确性并判断特征点数量是否充足，将正确性验证错误的特征点进行去除，并在特征点数量不足时，添加新的特征点。
4.根据权利要求1所述的一种融合定位方法，其特征在于，所述第一定位数据包括第一位置信息、第一速度信息和第一姿态信息；
所述步骤S2具体包括步骤：
S21、接收惯性测量单元的三轴的角速度、加速度和静止时的重力向量，并对预设时间内的所述角速度、所述加速度和所述重力向量进行积分，得到所述第一位置信息和所述第一速度信息，并根据四元数的更新得到所述第一姿态信息；
S22、根据所述第一位置信息、所述第一速度信息、所述第一姿态信息以及所述预设时间内所述单目摄像头估计的位置和姿态，建立位置误差方程和姿态误差方程；
S23、对所述位置误差方程和所述姿态误差方程进行优化求解，并根据计算结果对所述第一位置信息和所述第一姿态信息进行优化补偿。
5.根据权利要求1所述的一种融合定位方法，其特征在于，所述全球导航卫星***进行实时差分定位的解算时还包括步骤：根据采集的伪距、载波和多普勒观测值，得到接收机和卫星间的第一伪距双差模型和第一相位双差模型，并进行整周模糊度解；
所述步骤S3具体包括步骤：
S31、接收惯性导航***的第二定位数据，判断所述整周模糊度解中所述全球导航卫星***采集的所述伪距、所述载波和所述多普勒观测值是否存在因信号中断产生的缺失，若存在缺失则将所述第一伪距双差模型和所述第一相位双差模型在中断处根据所述第二定位数据进行线性化处理，并结合历史模糊度和协方差采用最小二乘法估计最优浮点模糊度解，采用LAMBDA方法计算得到整周模糊度解；
S32、根据所述整周模糊度解继续多频多模的实时差分定位的解算，最终得到所述第三定位数据。
6.根据权利要求5所述的一种融合定位方法，其特征在于，所述步骤S31和S32之间还包括步骤：
S311、对所述整周模糊度解进行正确性校验，根据全球导航卫星***信号正常时保存的历史整周模糊度解，进行假设检验统计量的统计分析，计算得到预设置信水平下的模糊度阈值；
S312、判断所述整周模糊度解是否落入所述模糊度阈值的范围内，若是则校验正确，否则校验错误，采用伪距测量的方法，计算整周模糊度解。
7.根据权利要求5所述的一种融合定位方法，其特征在于，所述第一定位数据包括第一位置信息、第一速度信息和第一姿态信息；
所述步骤S4具体包括步骤：
S41、根据所述第一位置信息构建第二伪距双差模型，并根据所述第一伪距双差模型和所述第二伪距双差模型分别计算得到第一双差伪距、第二双差伪距、第一双差伪距率和第二双差伪距率，根据所述第一双差伪距和所述第二双差伪距间的误差以及所述第一双差伪距率和所述第二双差伪距率间的误差作为数据融合的观测量，构造量测方程；
S42、根据所述第一位置信息、所述第一速度信息和所述第一姿态信息建立状态方程，所述状态方程包括位置方程、速度方程以及姿态方程。
8.根据权利要求7所述的一种融合定位方法，其特征在于，所述状态量误差包括速度误差、姿态误差以及位置误差；
所述步骤S5具体包括步骤：
S51、根据所述第一位置信息、所述第一速度信息、所述第一姿态信息、所述量测方程、位置方程、速度方程以及姿态方程，采用扩展卡尔曼滤波算法估计所述速度误差、所述姿态误差以及所述位置误差；
S52、根据所述位置误差、所述速度误差以及所述姿态误差分别对所述第一位置信息、所述第一速度信息以及所述第一姿态信息进行补偿优化，得到最终定位数据。
9.根据权利要求1所述的一种融合定位方法，其特征在于，所述步骤S5之后还包括步骤：
S6、将所述最终定位数据更新至所述惯性测量单元的所述第一定位数据。
10.一种融合定位终端，包括处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至9中任一所述的一种融合定位方法的步骤。
CN202210039694.4A 2022-01-12 2022-01-12 一种融合定位方法与终端 Active CN114396943B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210039694.4A CN114396943B (zh) 2022-01-12 2022-01-12 一种融合定位方法与终端

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210039694.4A CN114396943B (zh) 2022-01-12 2022-01-12 一种融合定位方法与终端

Publications (2)

Publication Number Publication Date
CN114396943A true CN114396943A (zh) 2022-04-26
CN114396943B CN114396943B (zh) 2024-07-23

Family

ID=81230523

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210039694.4A Active CN114396943B (zh) 2022-01-12 2022-01-12 一种融合定位方法与终端

Country Status (1)

Country Link
CN (1) CN114396943B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116182873A (zh) * 2023-05-04 2023-05-30 长沙驰芯半导体科技有限公司 室内定位方法、***及计算机可读介质
CN116681759A (zh) * 2023-04-19 2023-09-01 中国科学院上海微***与信息技术研究所 一种基于自监督视觉惯性里程计的相机位姿估计方法
CN117310756A (zh) * 2023-11-30 2023-12-29 宁波路特斯机器人有限公司 多传感器融合定位方法及***、机器可读存储介质

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040150557A1 (en) * 2003-01-21 2004-08-05 Ford Thomas John Inertial GPS navigation system with modified kalman filter
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
CN106597480A (zh) * 2016-12-08 2017-04-26 深圳大学 用于卫星导航rtk发射电台的抗干扰定位方法及***
CN107478221A (zh) * 2017-08-11 2017-12-15 黄润芳 一种用于移动终端的高精度定位方法
US20180188032A1 (en) * 2017-01-04 2018-07-05 Qualcomm Incorporated Systems and methods for using a global positioning system velocity in visual-inertial odometry
CN109212573A (zh) * 2018-10-15 2019-01-15 东南大学 一种城市峡谷环境下用于测绘车辆的定位***及方法
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109900265A (zh) * 2019-03-15 2019-06-18 武汉大学 一种camera/mems辅助北斗的机器人定位算法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及***
CN110412635A (zh) * 2019-07-22 2019-11-05 武汉大学 一种环境信标支持下的gnss/sins/视觉紧组合方法
CN111077556A (zh) * 2020-01-02 2020-04-28 东南大学 融合北斗与多传感器的机场行李牵引车定位装置与方法
CN111413720A (zh) * 2020-03-21 2020-07-14 哈尔滨工程大学 一种多频北斗载波相位差分/ins组合定位方法
CN112987065A (zh) * 2021-02-04 2021-06-18 东南大学 一种融合多传感器的手持式slam装置及其控制方法

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040150557A1 (en) * 2003-01-21 2004-08-05 Ford Thomas John Inertial GPS navigation system with modified kalman filter
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
CN106597480A (zh) * 2016-12-08 2017-04-26 深圳大学 用于卫星导航rtk发射电台的抗干扰定位方法及***
US20180188032A1 (en) * 2017-01-04 2018-07-05 Qualcomm Incorporated Systems and methods for using a global positioning system velocity in visual-inertial odometry
CN107478221A (zh) * 2017-08-11 2017-12-15 黄润芳 一种用于移动终端的高精度定位方法
CN109099912A (zh) * 2017-08-11 2018-12-28 黄润芳 室外精确定位导航方法、装置、电子设备及存储介质
CN109212573A (zh) * 2018-10-15 2019-01-15 东南大学 一种城市峡谷环境下用于测绘车辆的定位***及方法
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109900265A (zh) * 2019-03-15 2019-06-18 武汉大学 一种camera/mems辅助北斗的机器人定位算法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及***
CN110412635A (zh) * 2019-07-22 2019-11-05 武汉大学 一种环境信标支持下的gnss/sins/视觉紧组合方法
CN111077556A (zh) * 2020-01-02 2020-04-28 东南大学 融合北斗与多传感器的机场行李牵引车定位装置与方法
CN111413720A (zh) * 2020-03-21 2020-07-14 哈尔滨工程大学 一种多频北斗载波相位差分/ins组合定位方法
CN112987065A (zh) * 2021-02-04 2021-06-18 东南大学 一种融合多传感器的手持式slam装置及其控制方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
戴邵武;刘刚;王朕;张文广;李瑞涛;: "船用低成本GNSS测向***研究", 导航定位与授时, vol. 4, no. 02, 31 March 2017 (2017-03-31) *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116681759A (zh) * 2023-04-19 2023-09-01 中国科学院上海微***与信息技术研究所 一种基于自监督视觉惯性里程计的相机位姿估计方法
CN116681759B (zh) * 2023-04-19 2024-02-23 中国科学院上海微***与信息技术研究所 一种基于自监督视觉惯性里程计的相机位姿估计方法
CN116182873A (zh) * 2023-05-04 2023-05-30 长沙驰芯半导体科技有限公司 室内定位方法、***及计算机可读介质
CN117310756A (zh) * 2023-11-30 2023-12-29 宁波路特斯机器人有限公司 多传感器融合定位方法及***、机器可读存储介质
CN117310756B (zh) * 2023-11-30 2024-03-29 宁波路特斯机器人有限公司 多传感器融合定位方法及***、机器可读存储介质

Also Published As

Publication number Publication date
CN114396943B (zh) 2024-07-23

Similar Documents

Publication Publication Date Title
CN108519615B (zh) 基于组合导航和特征点匹配的移动机器人自主导航方法
CN114396943B (zh) 一种融合定位方法与终端
CN109931926B (zh) 一种基于站心坐标系的小型无人机无缝自主式导航方法
CN111578935B (zh) 一种利用惯导位置增量辅助gnss模糊度固定的方法
EP2434256B1 (en) Camera and inertial measurement unit integration with navigation data feedback for feature tracking
US20200124421A1 (en) Method and apparatus for estimating position
CN112230242B (zh) 位姿估计***和方法
EP2133662B1 (en) Methods and system of navigation using terrain features
CN106052691B (zh) 激光测距移动制图中闭合环误差纠正方法
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及***
CN113406682B (zh) 一种定位方法、装置、电子设备及存储介质
Georgy et al. Vehicle navigator using a mixture particle filter for inertial sensors/odometer/map data/GPS integration
CN104729506A (zh) 一种视觉信息辅助的无人机自主导航定位方法
CN114002725A (zh) 一种车道线辅助定位方法、装置、电子设备及存储介质
CN116184430B (zh) 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法
JP3900365B2 (ja) 測位装置および測位方法
Zou et al. A nonlinear transfer alignment of distributed POS based on adaptive second-order divided difference filter
Zhou et al. Online visual-inertial extrinsic calibration utilizing GNSS measurements for vehicle applications
Kanhere et al. Integrity for GPS/LiDAR fusion utilizing a RAIM framework
CN115435779A (zh) 一种基于gnss/imu/光流信息融合的智能***姿估计方法
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
Wang et al. GIVE: A tightly coupled RTK-inertial–visual state estimator for robust and precise positioning
Ćwian et al. GNSS-augmented lidar slam for accurate vehicle localization in large scale urban environments
CN107270904B (zh) 基于图像配准的无人机辅助引导控制***及方法
Dabove et al. Monocular visual odometry with unmanned underwater vehicle using low cost sensors

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