CN114088087A - 无人机gps-denied下高可靠高精度导航定位方法和系统 - Google Patents

无人机gps-denied下高可靠高精度导航定位方法和系统 Download PDF

Info

Publication number
CN114088087A
CN114088087A CN202210069955.7A CN202210069955A CN114088087A CN 114088087 A CN114088087 A CN 114088087A CN 202210069955 A CN202210069955 A CN 202210069955A CN 114088087 A CN114088087 A CN 114088087A
Authority
CN
China
Prior art keywords
aerial vehicle
unmanned aerial
positioning
represent
denied
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
CN202210069955.7A
Other languages
English (en)
Other versions
CN114088087B (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.)
Shenzhen University
Original Assignee
Shenzhen University
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 Shenzhen University filed Critical Shenzhen University
Priority to CN202210069955.7A priority Critical patent/CN114088087B/zh
Publication of CN114088087A publication Critical patent/CN114088087A/zh
Application granted granted Critical
Publication of CN114088087B publication Critical patent/CN114088087B/zh
Priority to PCT/CN2022/105069 priority patent/WO2023138007A1/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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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/20—Instruments for performing navigational calculations
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • G01S13/867—Combination of radar systems with 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/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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

本发明公开了无人机GPS‑DENIED下高可靠高精度导航定位方法和系统，所述方法包括：获取惯性传感器的测量值和双目相机的图像，提取图像的点特征并进行追踪，同时获取测距雷达测量得到的高度值；根据双目相机的视差和基线计算相机位姿和路标点的深度；将惯性传感器和双目相机的测量数据进行融合，并优化得到高精度的位姿数据；当检测到所述无人机在同一位置重复出现时，获取测距雷达的高度值，并加入检测到的关键帧后进行四自由度的位姿图优化；将优化后的位姿数据进行封装后形成伪GPS信号，并将所述伪GPS信号输入到无人机的进行定位。本发明通过将惯性传感器和双目相机的数据进行融合处理，高效准确提供定位信号，使无人机定位系统的安全性和鲁棒性大大提高。

Description

无人机GPS-DENIED下高可靠高精度导航定位方法和系统
技术领域
本发明涉及无人机GPS-DENIED下高可靠高精度导航定位技术领域，尤其涉及一种无人机GPS-DENIED下高可靠高精度导航定位方法、系统、无人机及计算机可读存储介质。
背景技术
随着无人机技术的不断的发展和成熟，无人机技术被广泛应用于航拍、农业、植保、快递运输、灾难救援、观察野生动物、监控传染病、测绘、新闻报道、电力巡检、救灾、影视拍摄等领域。然而，过度依赖GPS信号制肘了上述场景的进一步应用效果，例如，如果在高层建筑物遮挡，GPS定位精度下降或者没有GPS的室内等环境下，无人机完全无法实现自主飞行并完成指定的飞行任务，甚至发生坠机，或者人员受伤等情况，发展GPS信号匮乏情况下的定位手段迫在眉睫且拥有广阔的使用前景。
作为GPS信号匮乏情况下的定位补充手段，SLAM（simultaneous localizationand mapping，同步定位与建图）技术被广泛运用于机器人、无人机、无人驾驶、AR、VR等领域，依靠传感器可实现机器的自主定位、建图、路径规划等功能。由于传感器不同，SLAM的实现方式也有所不同，按传感器来分，SLAM主要包括激光SLAM和视觉SLAM两大类。其中，激光SLAM比视觉SLAM起步早，在理论、技术和产品落地上都相对成熟。早在2005年的时候，激光SLAM就已经被研究的比较透彻，理论框架也已初步确定。激光SLAM，是目前最稳定、最主流的定位导航方法。但是激光SLAM的成本相对较高，例如价格从几万到几十万不等，况且雷达体积、重量都较大，无法搭载于更灵活轻便的终端上，同时也影响美观和性能。另一方面，基于视觉的SLAM方案目前尚处于高速发展以及应用场景拓展、产品逐渐落地阶段，有非常良好地发展前景。视觉SLAM，因采用成本较小的视觉传感器，其成本会相对低很多，且体积小方便安装，而且在室内外环境下都能开展工作，视觉SLAM的缺点主要是环境的高度依赖，例如在暗处，光照变化比较大或者一些纹理稀疏，特征点比较少的环境中，会发生特征跟踪丢失，定位漂移等问题，同时还存在快速运动易丢失，单目视觉无法测量尺度，纯旋转问题无法估计等有待进一步研究解决的问题。
因此，现有技术还有待于改进和发展。
发明内容
本发明的主要目的在于提供一种无人机GPS-DENIED下高可靠高精度导航定位方法、系统、无人机及计算机可读存储介质，旨在解决现有技术中视觉SLAM在稀疏纹理、光线变化比较大的场景下定位漂移，以及单目视觉无法估计尺度的问题。
为实现上述目的，本发明提供一种无人机GPS-DENIED下高可靠高精度导航定位方法，所述无人机GPS-DENIED下高可靠高精度导航定位方法包括如下步骤：
获取所述惯性传感器的测量值和所述双目相机的图像，提取所述图像的点特征并进行追踪，同时获取所述测距雷达测量得到的高度值；
初始化所述双目相机，根据所述双目相机的视差和基线计算相机位姿和路标点的深度；
将所述惯性传感器和所述双目相机的测量数据进行融合，并采用基于滑动窗口的非线性图优化得到高精度的位姿数据；
当所述回环检测模块检测到所述无人机在同一位置重复出现时，获取所述测距雷达的高度值，并加入检测到的关键帧后进行四自由度的位姿图优化；
将优化后的位姿数据进行封装后形成伪GPS信号，并将所述伪GPS信号输入到所述无人机的进行定位，基于当前定位进行航线规划和设定航点任务。
可选地，所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其中，所述无人机GPS-DENIED下高可靠高精度导航定位方法还包括：
将所述惯性传感器采集的多个测量值进行积分得到所述无人机的位置、姿态和速度，积分公式如下：
Figure 984895DEST_PATH_IMAGE001
；
其中，
Figure 723044DEST_PATH_IMAGE002
表示IMU坐标系下的第k个关键帧时刻，
Figure 852805DEST_PATH_IMAGE003
表示
Figure 309195DEST_PATH_IMAGE002
到
Figure 799082DEST_PATH_IMAGE004
帧时刻之间位置的变化量，
Figure 391737DEST_PATH_IMAGE005
表示
Figure 207246DEST_PATH_IMAGE002
到
Figure 619773DEST_PATH_IMAGE004
帧时刻之间速度的变化量，
Figure 929663DEST_PATH_IMAGE006
表示
Figure 376825DEST_PATH_IMAGE002
到
Figure 363235DEST_PATH_IMAGE004
帧时刻之间姿态的变化量，
Figure 528637DEST_PATH_IMAGE007
表示世界坐标系到
Figure 360327DEST_PATH_IMAGE002
帧时刻IMU坐标系的旋转，
Figure 678307DEST_PATH_IMAGE008
表示
Figure 570040DEST_PATH_IMAGE004
帧时刻在世界坐标系的位置，
Figure 222738DEST_PATH_IMAGE009
表示
Figure 858119DEST_PATH_IMAGE002
帧时刻在世界坐标系的位置，
Figure 279873DEST_PATH_IMAGE010
表示世界坐标系下重力加速度，
Figure 608086DEST_PATH_IMAGE011
表示
Figure 233234DEST_PATH_IMAGE002
帧时刻到
Figure 406726DEST_PATH_IMAGE004
帧时刻的时间间隔，
Figure 948566DEST_PATH_IMAGE012
表示
Figure 447680DEST_PATH_IMAGE002
帧时刻在世界坐标系下的速度，
Figure 74971DEST_PATH_IMAGE013
表示
Figure 797027DEST_PATH_IMAGE004
帧时刻在世界坐标系下的速度，
Figure 458952DEST_PATH_IMAGE014
表示
Figure 863389DEST_PATH_IMAGE002
帧时刻在世界坐标系下的姿态，
Figure 446817DEST_PATH_IMAGE015
表示
Figure 493270DEST_PATH_IMAGE004
帧时刻在世界坐标系下的姿态，
Figure 744123DEST_PATH_IMAGE016
表示
Figure 70193DEST_PATH_IMAGE004
帧时刻IMU加速度计的偏置，
Figure 406497DEST_PATH_IMAGE017
表示
Figure 725483DEST_PATH_IMAGE002
帧时刻IMU加速度计的偏置，
Figure 830842DEST_PATH_IMAGE018
表示
Figure 842660DEST_PATH_IMAGE004
帧时刻IMU陀螺仪的偏置，
Figure 666260DEST_PATH_IMAGE019
表示
Figure 274090DEST_PATH_IMAGE002
帧时刻IMU陀螺仪的偏置。
可选地，所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其中，所述无人机GPS-DENIED下高可靠高精度导航定位方法还包括：
获取滑动窗口中的状态变量，所述状态变量包括：滑动窗口内的n+1个关键帧时刻的惯性传感器坐标系的位置、速度、姿态、加速度计偏置、陀螺仪偏置、双目相机到惯性传感器的外参和m+1个 3D 路标点的逆深度；
其中，待求解的状态变量表示公式如下：
Figure 765114DEST_PATH_IMAGE020
；
其中，
Figure 682254DEST_PATH_IMAGE021
表示要优化的n+1个关键帧时刻的状态量和m+1个路标点的集合，
Figure 727571DEST_PATH_IMAGE022
表示
Figure 653939DEST_PATH_IMAGE002
帧时刻世界坐标系下的位置
Figure 733890DEST_PATH_IMAGE023
、速度
Figure 572664DEST_PATH_IMAGE024
、姿态
Figure 105277DEST_PATH_IMAGE025
、IMU加速度计的偏置
Figure 835335DEST_PATH_IMAGE026
以及IMU陀螺仪的偏置
Figure 769793DEST_PATH_IMAGE027
的集合，
Figure 28736DEST_PATH_IMAGE028
表示相机坐标系到IMU坐标系的平移
Figure 314224DEST_PATH_IMAGE029
和旋转
Figure 333127DEST_PATH_IMAGE030
。
可选地，所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其中，图优化代价函数公式如下：
Figure 122091DEST_PATH_IMAGE031
Figure 817515DEST_PATH_IMAGE032
；
其中，
Figure 324719DEST_PATH_IMAGE033
表示边缘化的先验误差，
Figure 131001DEST_PATH_IMAGE034
表示海塞矩阵，
Figure 774472DEST_PATH_IMAGE035
表示IMU测量值的集合，
Figure 657109DEST_PATH_IMAGE036
表示IMU在
Figure 386030DEST_PATH_IMAGE002
到
Figure 996003DEST_PATH_IMAGE004
帧之间的的测量值，
Figure 759560DEST_PATH_IMAGE037
表示
Figure 796786DEST_PATH_IMAGE002
到
Figure 13004DEST_PATH_IMAGE004
帧的位置，
Figure 177400DEST_PATH_IMAGE038
表示预积分的误差，
Figure 795463DEST_PATH_IMAGE039
表示点特征的集合，
Figure 269170DEST_PATH_IMAGE040
表示第
Figure 972684DEST_PATH_IMAGE041
张图片中第
Figure 190038DEST_PATH_IMAGE042
个点特征的观测值，
Figure 662608DEST_PATH_IMAGE043
表示第
Figure 57948DEST_PATH_IMAGE041
张图片中的第
Figure 248758DEST_PATH_IMAGE042
个点特征的位置，
Figure 4225DEST_PATH_IMAGE044
表示点特征的重投影误差，
Figure 862459DEST_PATH_IMAGE045
表示线特征的集合，
Figure 146810DEST_PATH_IMAGE046
表示第
Figure 671306DEST_PATH_IMAGE047
张图片中第
Figure 964884DEST_PATH_IMAGE048
个点特征的观测值，
Figure 412046DEST_PATH_IMAGE049
表示第
Figure 398456DEST_PATH_IMAGE047
张图片中的第
Figure 298279DEST_PATH_IMAGE050
个线特征的位置，
Figure 129969DEST_PATH_IMAGE051
表示线特征的重投影误差。
可选地，所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其中，所述双目相机的视差为双目相机两个摄像头观察同一个目标所产生的方向差异，从被拍摄目标看两个摄像头之间的夹角，表示两个摄像头的视差角，两个摄像头之间的连线称作基线。
可选地，所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其中，所述相机位姿用于进行所述无人机导航，所述路标点的深度用于对环境进行建图。
可选地，所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其中，所述将所述惯性传感器和所述双目相机的测量数据进行融合，具体为：
将所述惯性传感器积分得到的位置、姿态、速度和所述双目相机提取的特征以及匹配得到的图像特征进行融合，得到优化后的位置、姿态和速度。
此外，为实现上述目的，本发明还提供一种无人机GPS-DENIED下高可靠高精度导航定位系统，其中，所述无人机GPS-DENIED下高可靠高精度导航定位系统包括：
数据获取模块，用于获取所述惯性传感器的测量值和所述双目相机的图像，提取所述图像的点特征并进行追踪，同时获取所述测距雷达测量得到的高度值；
数据计算模块，用于初始化所述双目相机，根据所述双目相机的视差和基线计算相机位姿和路标点的深度；
融合优化模块，用于将所述惯性传感器和所述双目相机的测量数据进行融合，并采用基于滑动窗口的非线性图优化得到高精度的位姿数据；
位姿优化模块，用于当所述回环检测模块检测到所述无人机在同一位置重复出现时，获取所述测距雷达的高度值，并加入检测到的关键帧后进行四自由度的位姿图优化；
定位导航模块，用于将优化后的位姿数据进行封装后形成伪GPS信号，并将所述伪GPS信号输入到所述无人机的进行定位，基于当前定位进行航线规划和设定航点任务。
此外，为实现上述目的，本发明还提供一种无人机，其中，所述无人机包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的无人机GPS-DENIED下高可靠高精度导航定位程序，所述无人机GPS-DENIED下高可靠高精度导航定位程序被所述处理器执行时实现如上所述的无人机GPS-DENIED下高可靠高精度导航定位方法的步骤。
此外，为实现上述目的，本发明还提供一种计算机可读存储介质，其中，所述计算机可读存储介质存储有无人机GPS-DENIED下高可靠高精度导航定位程序，所述无人机GPS-DENIED下高可靠高精度导航定位程序被处理器执行时实现如上所述的无人机GPS-DENIED下高可靠高精度导航定位方法的步骤。
本发明中，获取所述惯性传感器的测量值和所述双目相机的图像，提取所述图像的点特征并进行追踪，同时获取所述测距雷达测量得到的高度值；初始化所述双目相机，根据所述双目相机的视差和基线计算相机位姿和路标点的深度；将所述惯性传感器和所述双目相机的测量数据进行融合，并采用基于滑动窗口的非线性图优化得到高精度的位姿数据；当所述回环检测模块检测到所述无人机在同一位置重复出现时，获取所述测距雷达的高度值，并加入检测到的关键帧后进行四自由度的位姿图优化；将优化后的位姿数据进行封装后形成伪GPS信号，并将所述伪GPS信号输入到所述无人机的进行定位，基于当前定位进行航线规划和设定航点任务。本发明通过将惯性传感器和双目相机的数据进行融合处理，高效准确为无人机GPS-DENIED下高可靠高精度导航定位提供定位信号，使无人机GPS-DENIED下高可靠高精度导航定位系统的安全性和鲁棒性大大提高。
附图说明
图1是本发明无人机GPS-DENIED下高可靠高精度导航定位方法的较佳实施例的流程图；
图2是本发明无人机GPS-DENIED下高可靠高精度导航定位方法的较佳实施例中整个定位执行步骤的流程示意图；
图3是本发明无人机GPS-DENIED下高可靠高精度导航定位方法的较佳实施例中基于滑动窗口的紧耦合的视觉-惯性图优化的示意图；
图4是本发明无人机GPS-DENIED下高可靠高精度导航定位方法的较佳实施例中回环检测的位姿图优化的示意图；
图5是本发明无人机GPS-DENIED下高可靠高精度导航定位方法的较佳实施例中无人机飞行轨迹示意图；
图6是本发明无人机GPS-DENIED下高可靠高精度导航定位方法的较佳实施例中基于点线特征的无人机飞行轨迹示意图；
图7是本发明无人机GPS-DENIED下高可靠高精度导航定位系统的较佳实施例的原理示意图；
图8为本发明无人机的较佳实施例的运行环境示意图。
具体实施方式
为使本发明的目的、技术方案及优点更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本发明较佳实施例所述的无人机GPS-DENIED下高可靠高精度导航定位方法，如图1和图2所示，所述无人机GPS-DENIED下高可靠高精度导航定位方法包括以下步骤：
步骤S10、获取所述惯性传感器的测量值和所述双目相机的图像，提取所述图像的点特征并进行追踪，同时获取所述测距雷达测量得到的高度值。
具体地，本发明的硬件运算平台为：NVIDIA AGX Xavier，8-Core Carmel ARM，512Core Volta，32GB 256bit LPDDR4x；获取IMU（Inertial Measurement Unit，惯性传感器，200HZ，主要用来检测和测量加速度与旋转运动的传感器）的测量值，获取双目相机（例如深度相机为Intel Realsense D455，深度视场 86° × 57° （±3°））的图像；使用IMU_utils对D455的IMU进行标定，使用kalibr和Apriltag对D455双目相机的内参和相机和IMU之间的外参进行标定。视觉里程计的核心问题是如何根据图像估计相机运动。然而，图像本身是一个由亮度和色彩组成的矩阵，如果直接从矩阵层面考虑运动估计，将会非常困难。所以比较方便的做法是：从图像中提取比较有代表性的点特征和线特征并进行追踪，点特征采用Haris角点，使用KLT光流进行跟踪，线特征使用修改后的LSD（Line Segment Detector，线段检测器）算法去检测，LBD（Line Description Detector，线段描述器）来描述，使用KNN（K-Nearest Neighbor，K最邻近）来匹配，IMU部分则采用预积分的技术，将多个IMU的测量值进行积分得到系统的位置p、姿态q和速度v，如公式1所示；同时获取测距雷达测量得到的高度值。
即将所述惯性传感器采集的多个测量值进行积分得到所述无人机的位置、姿态和速度，积分公式如下：
Figure 713528DEST_PATH_IMAGE001
；（公式1）
其中，
Figure 605261DEST_PATH_IMAGE002
表示IMU坐标系下的第k个关键帧时刻，
Figure 992380DEST_PATH_IMAGE052
表示
Figure 893340DEST_PATH_IMAGE002
到
Figure 315094DEST_PATH_IMAGE004
帧时刻之间位置的变化量，
Figure 377728DEST_PATH_IMAGE053
表示
Figure 268455DEST_PATH_IMAGE002
到
Figure 441947DEST_PATH_IMAGE004
帧时刻之间速度的变化量，
Figure 718208DEST_PATH_IMAGE006
表示
Figure 217322DEST_PATH_IMAGE002
到
Figure 579033DEST_PATH_IMAGE004
帧时刻之间姿态的变化量，
Figure 556216DEST_PATH_IMAGE054
表示世界坐标系到
Figure 703295DEST_PATH_IMAGE002
帧时刻IMU坐标系的旋转，
Figure 107732DEST_PATH_IMAGE055
表示
Figure 222318DEST_PATH_IMAGE004
帧时刻在世界坐标系的位置，
Figure 3192DEST_PATH_IMAGE056
表示
Figure 988466DEST_PATH_IMAGE002
帧时刻在世界坐标系的位置，
Figure 563804DEST_PATH_IMAGE010
表示世界坐标系下重力加速度，
Figure 916419DEST_PATH_IMAGE011
表示
Figure 235405DEST_PATH_IMAGE002
帧时刻到
Figure 606343DEST_PATH_IMAGE004
帧时刻的时间间隔，
Figure 352582DEST_PATH_IMAGE057
表示
Figure 176182DEST_PATH_IMAGE002
帧时刻在世界坐标系下的速度，
Figure 49591DEST_PATH_IMAGE058
表示
Figure 9457DEST_PATH_IMAGE004
帧时刻在世界坐标系下的速度，
Figure 192176DEST_PATH_IMAGE059
表示
Figure 503072DEST_PATH_IMAGE002
帧时刻在世界坐标系下的姿态，
Figure 163861DEST_PATH_IMAGE060
表示
Figure 243812DEST_PATH_IMAGE004
帧时刻在世界坐标系下的姿态，
Figure 348165DEST_PATH_IMAGE061
表示
Figure 880778DEST_PATH_IMAGE004
帧时刻IMU加速度计的偏置，
Figure 345257DEST_PATH_IMAGE062
表示
Figure 279715DEST_PATH_IMAGE002
帧时刻IMU加速度计的偏置，
Figure 804237DEST_PATH_IMAGE063
表示
Figure 824146DEST_PATH_IMAGE004
帧时刻IMU陀螺仪的偏置，
Figure 826737DEST_PATH_IMAGE064
表示
Figure 632013DEST_PATH_IMAGE002
帧时刻IMU陀螺仪的偏置。
步骤S20、初始化所述双目相机，根据所述双目相机的视差和基线计算相机位姿和路标点的深度。
具体地，首先进行双目相机的初始化，双目相机存在视差，视差就是从有一定距离的两个点上即两个摄像头，观察同一个目标所产生的方向差异，从目标看两个点之间的夹角，叫做这两个点的视差角，两点之间的连线称作基线。只要知道视差角度和基线长度，就可以计算出目标和观测者之间的距离。根据双目相机的视差和基线计算出相机位姿和路标点即图像特征的深度。进而可以利用计算出的相机的位姿来进行无人机的导航，路标点的深度来对环境进行建图。因为陀螺仪也可以积分出相机的旋转，所以可以通过相机计算出来的旋转等于IMU积分出来的旋转，这一等式计算出IMU的偏差，然后通过计算出来的偏差对先前的IMU进行重新积分，再通过将IMU和相机计算出来的位姿进行一步优化。最终能够得到一个无人机当前时刻不太准确的位姿，作为后端非线性优化的初值。
本发明的后端采用了滑动窗口的形式， 将点特征和线特征一起进行优化，滑动窗口中的状态变量包括滑动窗口内的n+1个关键帧时刻 IMU 坐标系的位置、速度、姿态（旋转）、加速度计偏置、陀螺仪偏置， Camera 到 IMU 的外参， m +1个 3D 路标点的逆深度，待求解的状态变量表示如公式2所示：
Figure 327437DEST_PATH_IMAGE020
；（公式2）
其中，
Figure 100221DEST_PATH_IMAGE021
表示要优化的n+1个关键帧时刻的状态量和m+1个路标点的集合，
Figure 640924DEST_PATH_IMAGE022
表示
Figure 815553DEST_PATH_IMAGE002
帧时刻世界坐标系下的位置
Figure 426751DEST_PATH_IMAGE023
、速度
Figure 421252DEST_PATH_IMAGE065
、姿态
Figure 31224DEST_PATH_IMAGE066
、IMU加速度计的偏置
Figure 794781DEST_PATH_IMAGE026
以及IMU陀螺仪的偏置
Figure 832007DEST_PATH_IMAGE027
的集合，
Figure 48225DEST_PATH_IMAGE028
表示相机坐标系到IMU坐标系的平移
Figure 212621DEST_PATH_IMAGE029
和旋转
Figure 830684DEST_PATH_IMAGE030
。
图优化代价函数如公式3所示：
Figure 38812DEST_PATH_IMAGE031
Figure 7905DEST_PATH_IMAGE032
；（公式3）
其中，
Figure 694101DEST_PATH_IMAGE033
表示边缘化的先验误差，
Figure 432250DEST_PATH_IMAGE034
表示海塞矩阵，
Figure 827590DEST_PATH_IMAGE035
表示IMU测量值的集合，
Figure 18400DEST_PATH_IMAGE036
表示IMU在
Figure 508287DEST_PATH_IMAGE002
到
Figure 835364DEST_PATH_IMAGE004
帧之间的的测量值，
Figure 650873DEST_PATH_IMAGE037
表示
Figure 328979DEST_PATH_IMAGE002
到
Figure 373289DEST_PATH_IMAGE004
帧的位置，
Figure 86030DEST_PATH_IMAGE038
表示预积分的误差，
Figure 541283DEST_PATH_IMAGE039
表示点特征的集合，
Figure 706685DEST_PATH_IMAGE040
表示第
Figure 538374DEST_PATH_IMAGE041
张图片中第
Figure 371201DEST_PATH_IMAGE042
个点特征的观测值，
Figure 748087DEST_PATH_IMAGE067
表示第
Figure 400785DEST_PATH_IMAGE041
张图片中的第
Figure 301745DEST_PATH_IMAGE042
个点特征的位置，
Figure 457920DEST_PATH_IMAGE044
表示点特征的重投影误差，
Figure 520554DEST_PATH_IMAGE045
表示线特征的集合，
Figure 394969DEST_PATH_IMAGE068
表示第
Figure 850352DEST_PATH_IMAGE047
张图片中第
Figure 126613DEST_PATH_IMAGE048
个点特征的观测值，
Figure 360148DEST_PATH_IMAGE069
表示第
Figure 987438DEST_PATH_IMAGE047
张图片中的第
Figure 964622DEST_PATH_IMAGE050
个线特征的位置，
Figure 95389DEST_PATH_IMAGE070
表示线特征的重投影误差。图优化的结构表示如图3所示。
步骤S30、将所述惯性传感器和所述双目相机的测量数据进行融合，并采用基于滑动窗口的非线性图优化得到高精度的位姿数据。
具体地，系统前端采用紧耦合的方式，通过将IMU数据积分得到的位置、速度、姿态和双目相机的特征提取和匹配得到的图像特征进行融合，得到优化后的位置、速度、姿态；后端采用基于滑动窗口的非线性图优化对其进行进一步的优化，得到了高精度的位姿数据。
步骤S40、当所述回环检测模块检测到所述无人机在同一位置重复出现时，获取所述测距雷达的高度值，并加入检测到的关键帧后进行四自由度的位姿图优化。
具体地，所述回环检测模块会以另外一个线程利用带BRIEF描述符的词袋模型来执行循环检测，检测无人机是否在同一位置重复出现；当检测到循环时，需要通过检索到的特征在新关键帧及其循环候选之间建立连接。如图4所示，一旦检测到重定位就会首先去获取雷达测量的高度数据，紧接着将回环检测的关键帧加入到图优化中进行4自由度（x，y，z，yaw）的位姿图优化。因为雷达的数据会更加精确，这在进行全局优化的时候，能够对其他3个自由度的数据进行优化，得到更加准确的定位效果。
步骤S50、将优化后的位姿数据进行封装后形成伪GPS信号，并将所述伪GPS信号输入到所述无人机的进行定位，基于当前定位进行航线规划和设定航点任务。
具体地，所述无人机上的脑件（飞控）将通过串口将SLAM输出的位姿数据，通过MavLink（Micro Air Vehicle Message Marshalling Library，微型空中飞行器链路通讯协议）协议进行封装形成伪GPS信号，输入到飞控中来给无人机进行定位。基于该定位，可以进行相应的航线规划，设定航点任务。
如图5所示，通过SLAM输出的位姿给飞控，控制无人机在空中飞行一圈后，回到原点，轨迹图展示了飞机的起飞点和落地点是一样的。
如图6所示，是基于点线特征VINS算法运行Euroc数据集的结果展示图；对比单纯的点特征的VINS-FUSION，本发明们的算法的定位精度能够达到更高。
本发明将线特征集成到了双目-惯性SLAM中，同时融合了激光的数据来进行联合优化，这样能够有效降低其他轴的数据误差，提高了系统在稀疏纹理以及光照变化比较大的场景中定位的准确性。同时开发了一种当SLAM失效时的重启机制，能够大大加强无人机使用SLAM进行定位的安全性。
进一步地，将线特征加入到双目相机来使用，融合进VINS，使用其他测高的模块来测量无人机距地面的高度，或者在可行的场景下同时测量多轴的数据，来共同参与无人机位姿图的联合优化。
本发明技术方案所达到的技术效果如下：
（1）、融合双目相机和IMU的测量数据，形成高效准确的位姿估算算法：IMU的数据有响应灵敏的特点，IMU测量得到的角速度和加速度长时间积分计算位置、姿态以及速度会发散，精度会下降，且IMU存在零偏。在静置状态下采集一段角速度数据，理想状态下
Figure 516137DEST_PATH_IMAGE071
，但是实际上陀螺仪的输出是一条复合白噪声信号缓慢变化的曲线，曲线的平均值就是零偏值。因为零偏的存在，估计的位姿误差会越来越大。因此，IMU适合计算短时间、快速的运动；而视觉定位模块采用的是双目相机具有静态精度较好，但是存在测量死区且反应较慢的特点，适合计算长时间、慢速的运动。本发明通过传感器融合的新型框架，将IMU和双目相机进行传感器融合处理，各取所长，形成高效、稳定的定位算法。
（2）、当前主流SLAM算法都是完全基于点特征的，在纹理比较稀疏或者重复纹理的场景下，SLAM算法表现会不佳。本发明通过引入线特征机制，SLAM前端在提取点特征的同时，提取线特征。因为线特征具有更多的几何信息，并且对光照变化的鲁棒性也会更好，这将大大提高SLAM算法的准确性。同时采用更好的后端优化方法进行多传感器融合，例如，使用更加精准的激光测距传感器来测量高度，然后利用z轴的高度信息，来优化其他轴的数据。
（3）、开发了独有的安全机制，考虑到SLAM算法普遍存在的失效问题，系统能保存失效前的状态变量并快速初始化，并计算失效时间的位姿变化进行补偿，使整套定位系统的安全性和鲁棒性大大提高。
进一步地，如图7所示，基于上述无人机GPS-DENIED下高可靠高精度导航定位方法，本发明还相应提供了一种无人机GPS-DENIED下高可靠高精度导航定位系统，其中，所述无人机GPS-DENIED下高可靠高精度导航定位系统包括：
数据获取模块51，用于获取所述惯性传感器的测量值和所述双目相机的图像，提取所述图像的点特征并进行追踪，同时获取所述测距雷达测量得到的高度值；
数据计算模块52，用于初始化所述双目相机，根据所述双目相机的视差和基线计算相机位姿和路标点的深度；
融合优化模块53，用于将所述惯性传感器和所述双目相机的测量数据进行融合，并采用基于滑动窗口的非线性图优化得到高精度的位姿数据；
位姿优化模块54，用于当所述回环检测模块检测到所述无人机在同一位置重复出现时，获取所述测距雷达的高度值，并加入检测到的关键帧后进行四自由度的位姿图优化；
定位导航模块55，用于将优化后的位姿数据进行封装后形成伪GPS信号，并将所述伪GPS信号输入到所述无人机的进行定位，基于当前定位进行航线规划和设定航点任务。
进一步地，如图8所示，基于上述无人机GPS-DENIED下高可靠高精度导航定位方法和系统，本发明还相应提供了一种无人机，所述无人机包括处理器10、存储器20及显示器30。图8仅示出了无人机的部分组件，但是应理解的是，并不要求实施所有示出的组件，可以替代的实施更多或者更少的组件。
所述存储器20在一些实施例中可以是所述无人机的内部存储单元，例如无人机的硬盘或内存。所述存储器20在另一些实施例中也可以是所述无人机的外部存储设备，例如所述无人机上配备的插接式硬盘，智能存储卡（Smart Media Card, SMC），安全数字（Secure Digital, SD）卡，闪存卡（Flash Card）等。进一步地，所述存储器20还可以既包括所述无人机的内部存储单元也包括外部存储设备。所述存储器20用于存储安装于所述无人机的应用软件及各类数据，例如所述安装无人机的程序代码等。所述存储器20还可以用于暂时地存储已经输出或者将要输出的数据。在一实施例中，存储器20上存储有无人机GPS-DENIED下高可靠高精度导航定位程序40，该无人机GPS-DENIED下高可靠高精度导航定位程序40可被处理器10所执行，从而实现本申请中无人机GPS-DENIED下高可靠高精度导航定位方法。
所述处理器10在一些实施例中可以是一中央处理器（Central Processing Unit,CPU），微处理器或其他数据处理芯片，用于运行所述存储器20中存储的程序代码或处理数据，例如执行所述无人机GPS-DENIED下高可靠高精度导航定位方法等。
所述显示器30在一些实施例中可以是LED显示器、液晶显示器、触控式液晶显示器以及OLED（Organic Light-Emitting Diode，有机发光二极管）触摸器等。所述显示器30用于显示在所述无人机的信息以及用于显示可视化的用户界面。所述无人机的部件10-30通过系统总线相互通信。
在一实施例中，当处理器10执行所述存储器20中无人机GPS-DENIED下高可靠高精度导航定位程序40时实现所述无人机GPS-DENIED下高可靠高精度导航定位方法的步骤。
本发明还提供一种计算机可读存储介质，其中，所述计算机可读存储介质存储有无人机GPS-DENIED下高可靠高精度导航定位程序，所述无人机GPS-DENIED下高可靠高精度导航定位程序被处理器执行时实现如上所述的无人机GPS-DENIED下高可靠高精度导航定位方法的步骤。
综上所述，本发明提供一种无人机GPS-DENIED下高可靠高精度导航定位方法、系统、无人机及计算机可读存储介质，所述方法包括：获取所述惯性传感器的测量值和所述双目相机的图像，提取所述图像的点特征并进行追踪，同时获取所述测距雷达测量得到的高度值；初始化所述双目相机，根据所述双目相机的视差和基线计算相机位姿和路标点的深度；将所述惯性传感器和所述双目相机的测量数据进行融合，并采用基于滑动窗口的非线性图优化得到高精度的位姿数据；当所述回环检测模块检测到所述无人机在同一位置重复出现时，获取所述测距雷达的高度值，并加入检测到的关键帧后进行四自由度的位姿图优化；将优化后的位姿数据进行封装后形成伪GPS信号，并将所述伪GPS信号输入到所述无人机的进行定位，基于当前定位进行航线规划和设定航点任务。本发明通过将惯性传感器和双目相机的数据进行融合处理，高效准确为无人机GPS-DENIED下高可靠高精度导航定位提供定位信号，使无人机GPS-DENIED下高可靠高精度导航定位系统的安全性和鲁棒性大大提高。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者无人机不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者无人机所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者无人机中还存在另外的相同要素。
当然，本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关硬件（如处理器，控制器等）来完成，所述的程序可存储于一计算机可读取的计算机可读存储介质中，所述程序在执行时可包括如上述各方法实施例的流程。其中所述的计算机可读存储介质可为存储器、磁碟、光盘等。
应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (10)

1.一种无人机GPS-DENIED下高可靠高精度导航定位方法，其特征在于，所述无人机包括惯性传感器、双目相机、测距雷达和回环检测模块，所述无人机GPS-DENIED下高可靠高精度导航定位方法包括：
获取所述惯性传感器的测量值和所述双目相机的图像，提取所述图像的点特征并进行追踪，同时获取所述测距雷达测量得到的高度值；
初始化所述双目相机，根据所述双目相机的视差和基线计算相机位姿和路标点的深度；
将所述惯性传感器和所述双目相机的测量数据进行融合，并采用基于滑动窗口的非线性图优化得到高精度的位姿数据；
当所述回环检测模块检测到所述无人机在同一位置重复出现时，获取所述测距雷达的高度值，并加入检测到的关键帧后进行四自由度的位姿图优化；
将优化后的位姿数据进行封装后形成伪GPS信号，并将所述伪GPS信号输入到所述无人机的进行定位，基于当前定位进行航线规划和设定航点任务。
2.根据权利要求1所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其特征在于，所述无人机GPS-DENIED下高可靠高精度导航定位方法还包括：
将所述惯性传感器采集的多个测量值进行积分得到所述无人机的位置、姿态和速度，积分公式如下：
Figure 362382DEST_PATH_IMAGE001
；
其中，
Figure 143257DEST_PATH_IMAGE002
表示IMU坐标系下的第k个关键帧时刻，
Figure 659689DEST_PATH_IMAGE003
表示
Figure 235026DEST_PATH_IMAGE002
到
Figure 836909DEST_PATH_IMAGE004
帧时刻之间位置的变化量，
Figure 172207DEST_PATH_IMAGE005
表示
Figure 277566DEST_PATH_IMAGE002
到
Figure 289384DEST_PATH_IMAGE004
帧时刻之间速度的变化量，
Figure 112984DEST_PATH_IMAGE006
表示
Figure 251972DEST_PATH_IMAGE002
到
Figure 211838DEST_PATH_IMAGE004
帧时刻之间姿态的变化量，
Figure 394558DEST_PATH_IMAGE007
表示世界坐标系到
Figure 971032DEST_PATH_IMAGE002
帧时刻IMU坐标系的旋转，
Figure 631821DEST_PATH_IMAGE008
表示
Figure 462505DEST_PATH_IMAGE004
帧时刻在世界坐标系的位置，
Figure 81705DEST_PATH_IMAGE009
表示
Figure 614317DEST_PATH_IMAGE002
帧时刻在世界坐标系的位置，
Figure 344376DEST_PATH_IMAGE010
表示世界坐标系下重力加速度，
Figure 278834DEST_PATH_IMAGE011
表示
Figure 554089DEST_PATH_IMAGE002
帧时刻到
Figure 839577DEST_PATH_IMAGE004
帧时刻的时间间隔，
Figure 842168DEST_PATH_IMAGE012
表示
Figure 896711DEST_PATH_IMAGE002
帧时刻在世界坐标系下的速度，
Figure 857714DEST_PATH_IMAGE013
表示
Figure 850072DEST_PATH_IMAGE004
帧时刻在世界坐标系下的速度，
Figure 921933DEST_PATH_IMAGE014
表示
Figure 830983DEST_PATH_IMAGE015
帧时刻在世界坐标系下的姿态，
Figure 962887DEST_PATH_IMAGE016
表示
Figure 973700DEST_PATH_IMAGE004
帧时刻在世界坐标系下的姿态，
Figure 583673DEST_PATH_IMAGE017
表示
Figure 612809DEST_PATH_IMAGE004
帧时刻IMU加速度计的偏置，
Figure 384456DEST_PATH_IMAGE018
表示
Figure 131832DEST_PATH_IMAGE002
帧时刻IMU加速度计的偏置，
Figure 24789DEST_PATH_IMAGE019
表示
Figure 908432DEST_PATH_IMAGE004
帧时刻IMU陀螺仪的偏置，
Figure 116559DEST_PATH_IMAGE020
表示
Figure 85652DEST_PATH_IMAGE002
帧时刻IMU陀螺仪的偏置。
3.根据权利要求2所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其特征在于，所述无人机GPS-DENIED下高可靠高精度导航定位方法还包括：
获取滑动窗口中的状态变量，所述状态变量包括：滑动窗口内的n+1个关键帧时刻的惯性传感器坐标系的位置、速度、姿态、加速度计偏置、陀螺仪偏置、双目相机到惯性传感器的外参和m+1个 3D 路标点的逆深度；
其中，待求解的状态变量表示公式如下：
Figure 37428DEST_PATH_IMAGE021
；
其中，
Figure 791888DEST_PATH_IMAGE022
表示要优化的n+1个关键帧时刻的状态量和m+1个路标点的集合，
Figure 905338DEST_PATH_IMAGE023
表示
Figure 361727DEST_PATH_IMAGE024
帧时刻世界坐标系下的位置
Figure 117193DEST_PATH_IMAGE025
、速度
Figure 709849DEST_PATH_IMAGE026
、姿态
Figure 276090DEST_PATH_IMAGE027
、IMU加速度计的偏置
Figure 219775DEST_PATH_IMAGE028
以及IMU陀螺仪的偏置
Figure 247774DEST_PATH_IMAGE029
的集合，
Figure 960515DEST_PATH_IMAGE030
表示相机坐标系到IMU坐标系的平移
Figure 681347DEST_PATH_IMAGE031
和旋转
Figure 863061DEST_PATH_IMAGE032
。
4.根据权利要求3所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其特征在于，图优化代价函数公式如下：
Figure 694750DEST_PATH_IMAGE033
Figure 261998DEST_PATH_IMAGE034
；
其中，
Figure 419310DEST_PATH_IMAGE035
表示边缘化的先验误差，
Figure 72008DEST_PATH_IMAGE036
表示海塞矩阵，
Figure 458121DEST_PATH_IMAGE037
表示IMU测量值的集合，
Figure 879875DEST_PATH_IMAGE038
表示IMU在
Figure 208088DEST_PATH_IMAGE024
到
Figure 348083DEST_PATH_IMAGE039
帧之间的的测量值，
Figure 803466DEST_PATH_IMAGE040
表示
Figure 345306DEST_PATH_IMAGE024
到
Figure 578841DEST_PATH_IMAGE039
帧的位置，
Figure 206131DEST_PATH_IMAGE041
表示预积分的误差，
Figure 917735DEST_PATH_IMAGE042
表示点特征的集合，
Figure 330393DEST_PATH_IMAGE043
表示第
Figure 409DEST_PATH_IMAGE044
张图片中第
Figure 849416DEST_PATH_IMAGE045
个点特征的观测值，
Figure 630291DEST_PATH_IMAGE046
表示第
Figure 146722DEST_PATH_IMAGE044
张图片中的第
Figure 738372DEST_PATH_IMAGE045
个点特征的位置，
Figure 74675DEST_PATH_IMAGE047
表示点特征的重投影误差，
Figure 659240DEST_PATH_IMAGE048
表示线特征的集合，
Figure 30179DEST_PATH_IMAGE049
表示第
Figure 776418DEST_PATH_IMAGE050
张图片中第
Figure 344891DEST_PATH_IMAGE051
个点特征的观测值，
Figure 467567DEST_PATH_IMAGE052
表示第
Figure 693012DEST_PATH_IMAGE050
张图片中的第
Figure 610153DEST_PATH_IMAGE053
个线特征的位置，
Figure 186628DEST_PATH_IMAGE054
表示线特征的重投影误差。
5.根据权利要求1所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其特征在于，所述双目相机的视差为双目相机两个摄像头观察同一个目标所产生的方向差异，从被拍摄目标看两个摄像头之间的夹角，表示两个摄像头的视差角，两个摄像头之间的连线称作基线。
6.根据权利要求1所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其特征在于，所述相机位姿用于进行所述无人机导航，所述路标点的深度用于对环境进行建图。
7.根据权利要求2所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其特征在于，所述将所述惯性传感器和所述双目相机的测量数据进行融合，具体为：
将所述惯性传感器积分得到的位置、姿态、速度和所述双目相机提取的特征以及匹配得到的图像特征进行融合，得到优化后的位置、姿态和速度。
8.一种无人机GPS-DENIED下高可靠高精度导航定位系统，其特征在于，所述无人机GPS-DENIED下高可靠高精度导航定位系统包括：
数据获取模块，用于获取惯性传感器的测量值和双目相机的图像，提取所述图像的点特征并进行追踪，同时获取测距雷达测量得到的高度值；
数据计算模块，用于初始化所述双目相机，根据所述双目相机的视差和基线计算相机位姿和路标点的深度；
融合优化模块，用于将所述惯性传感器和所述双目相机的测量数据进行融合，并采用基于滑动窗口的非线性图优化得到高精度的位姿数据；
位姿优化模块，用于当回环检测模块检测到所述无人机在同一位置重复出现时，获取所述测距雷达的高度值，并加入检测到的关键帧后进行四自由度的位姿图优化；
定位导航模块，用于将优化后的位姿数据进行封装后形成伪GPS信号，并将所述伪GPS信号输入到所述无人机的进行定位，基于当前定位进行航线规划和设定航点任务。
9.一种无人机，其特征在于，所述无人机包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的无人机GPS-DENIED下高可靠高精度导航定位程序，所述无人机GPS-DENIED下高可靠高精度导航定位程序被所述处理器执行时实现如权利要求1-7任一项所述的无人机GPS-DENIED下高可靠高精度导航定位方法的步骤。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有无人机GPS-DENIED下高可靠高精度导航定位程序，所述无人机GPS-DENIED下高可靠高精度导航定位程序被处理器执行时实现如权利要求1-7任一项所述的无人机GPS-DENIED下高可靠高精度导航定位方法的步骤。
CN202210069955.7A 2022-01-21 2022-01-21 无人机gps-denied下高可靠高精度导航定位方法和系统 Active CN114088087B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202210069955.7A CN114088087B (zh) 2022-01-21 2022-01-21 无人机gps-denied下高可靠高精度导航定位方法和系统
PCT/CN2022/105069 WO2023138007A1 (zh) 2022-01-21 2022-07-12 无人机gps-denied下高可靠高精度导航定位方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210069955.7A CN114088087B (zh) 2022-01-21 2022-01-21 无人机gps-denied下高可靠高精度导航定位方法和系统

Publications (2)

Publication Number Publication Date
CN114088087A true CN114088087A (zh) 2022-02-25
CN114088087B CN114088087B (zh) 2022-04-15

Family

ID=80309056

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210069955.7A Active CN114088087B (zh) 2022-01-21 2022-01-21 无人机gps-denied下高可靠高精度导航定位方法和系统

Country Status (2)

Country Link
CN (1) CN114088087B (zh)
WO (1) WO2023138007A1 (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023138007A1 (zh) * 2022-01-21 2023-07-27 深圳大学 无人机gps-denied下高可靠高精度导航定位方法和系统

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117148871B (zh) * 2023-11-01 2024-02-27 中国民航管理干部学院 一种多无人机协同电力巡检方法及系统
CN117437563B (zh) * 2023-12-13 2024-03-15 黑龙江惠达科技股份有限公司 一种基于双目视觉的植保无人机打点方法、装置及设备
CN117739996B (zh) * 2024-02-21 2024-04-30 西北工业大学 一种基于事件相机惯性紧耦合的自主定位方法
CN118052869A (zh) * 2024-04-15 2024-05-17 深圳市峰和数智科技有限公司 无人机位姿参数优化方法、装置、存储介质及计算机设备
CN118089704A (zh) * 2024-04-25 2024-05-28 广汽埃安新能源汽车股份有限公司 一种基于自适应特征提取的slam方法及装置

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107909614A (zh) * 2017-11-13 2018-04-13 中国矿业大学 一种gps失效环境下巡检机器人定位方法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及系统
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN111024066A (zh) * 2019-12-10 2020-04-17 中国航空无线电电子研究所 一种无人机视觉-惯性融合室内定位方法
CN111595333A (zh) * 2020-04-26 2020-08-28 武汉理工大学 视觉惯性激光数据融合的模块化无人车定位方法及系统
CN111932674A (zh) * 2020-06-30 2020-11-13 博雅工道（北京）机器人科技有限公司 一种线激光视觉惯性系统的优化方法
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN112240768A (zh) * 2020-09-10 2021-01-19 西安电子科技大学 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法
CN112258600A (zh) * 2020-10-19 2021-01-22 浙江大学 一种基于视觉与激光雷达的同时定位与地图构建方法
US20210080287A1 (en) * 2019-09-18 2021-03-18 Harbin Engineering University Method for initial alignment of radar assisted airborne strapdown inertial navigation system
CN112634451A (zh) * 2021-01-11 2021-04-09 福州大学 一种融合多传感器的室外大场景三维建图方法
CN112802196A (zh) * 2021-02-01 2021-05-14 北京理工大学 基于点线特征融合的双目惯性同时定位与地图构建方法
CN113192140A (zh) * 2021-05-25 2021-07-30 华中科技大学 一种基于点线特征的双目视觉惯性定位方法和系统

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105892489B (zh) * 2016-05-24 2019-09-10 国网山东省电力公司电力科学研究院 一种基于多传感器融合的自主避障无人机系统及控制方法
US11422253B2 (en) * 2018-11-19 2022-08-23 Tdk Corportation Method and system for positioning using tightly coupled radar, motion sensors and map information
CN109900265A (zh) * 2019-03-15 2019-06-18 武汉大学 一种camera/mems辅助北斗的机器人定位算法
CN113625774B (zh) * 2021-09-10 2023-07-21 天津大学 局部地图匹配与端到端测距多无人机协同定位系统和方法
CN114088087B (zh) * 2022-01-21 2022-04-15 深圳大学 无人机gps-denied下高可靠高精度导航定位方法和系统

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107909614A (zh) * 2017-11-13 2018-04-13 中国矿业大学 一种gps失效环境下巡检机器人定位方法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及系统
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
US20210080287A1 (en) * 2019-09-18 2021-03-18 Harbin Engineering University Method for initial alignment of radar assisted airborne strapdown inertial navigation system
CN111024066A (zh) * 2019-12-10 2020-04-17 中国航空无线电电子研究所 一种无人机视觉-惯性融合室内定位方法
CN111595333A (zh) * 2020-04-26 2020-08-28 武汉理工大学 视觉惯性激光数据融合的模块化无人车定位方法及系统
CN111932674A (zh) * 2020-06-30 2020-11-13 博雅工道（北京）机器人科技有限公司 一种线激光视觉惯性系统的优化方法
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN112240768A (zh) * 2020-09-10 2021-01-19 西安电子科技大学 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法
CN112258600A (zh) * 2020-10-19 2021-01-22 浙江大学 一种基于视觉与激光雷达的同时定位与地图构建方法
CN112634451A (zh) * 2021-01-11 2021-04-09 福州大学 一种融合多传感器的室外大场景三维建图方法
CN112802196A (zh) * 2021-02-01 2021-05-14 北京理工大学 基于点线特征融合的双目惯性同时定位与地图构建方法
CN113192140A (zh) * 2021-05-25 2021-07-30 华中科技大学 一种基于点线特征的双目视觉惯性定位方法和系统

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023138007A1 (zh) * 2022-01-21 2023-07-27 深圳大学 无人机gps-denied下高可靠高精度导航定位方法和系统

Also Published As

Publication number Publication date
CN114088087B (zh) 2022-04-15
WO2023138007A1 (zh) 2023-07-27

Similar Documents

Publication Publication Date Title
CN114088087B (zh) 无人机gps-denied下高可靠高精度导航定位方法和系统
CN109084732B (zh) 定位与导航方法、装置及处理设备
CN109887057B (zh) 生成高精度地图的方法和装置
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN111156998B (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
CN109506642B (zh) 一种机器人多相机视觉惯性实时定位方法及装置
CN110044354A (zh) 一种双目视觉室内定位与建图方法及装置
CN109885080B (zh) 自主控制系统及自主控制方法
CN109520497A (zh) 基于视觉和imu的无人机自主定位方法
CN107909614B (zh) 一种gps失效环境下巡检机器人定位方法
CN112116651B (zh) 一种基于无人机单目视觉的地面目标定位方法和系统
Lin et al. R $^ 3$ LIVE++: A Robust, Real-time, Radiance reconstruction package with a tightly-coupled LiDAR-Inertial-Visual state Estimator
CN108613675B (zh) 低成本无人飞行器移动测量方法及系统
Hinzmann et al. Flexible stereo: constrained, non-rigid, wide-baseline stereo vision for fixed-wing aerial platforms
CN116007609A (zh) 一种多光谱图像和惯导融合的定位方法和计算系统
CN114018254B (zh) 一种激光雷达与旋转惯导一体化构架与信息融合的slam方法
CN114821363A (zh) 一种基于语义信息匹配的无人机定位建图方法及系统
CN116989772B (zh) 一种空地多模态多智能体协同定位与建图方法
CN105807083A (zh) 一种无人飞行器实时测速方法及系统
Ling et al. RGB-D inertial odometry for indoor robot via keyframe-based nonlinear optimization
CN116659490A (zh) 低成本视觉-惯性融合的slam方法
Aminzadeh et al. Implementation and performance evaluation of optical flow navigation system under specific conditions for a flying robot
Cao et al. Visual-Inertial-Laser SLAM Based on ORB-SLAM3
Hu et al. Efficient Visual-Inertial navigation with point-plane map
Xu et al. A Multi-source Information Fusion Method for Mobile Robot Visual-inertial Navigation

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