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

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

Info

Publication number
CN114088087B
CN114088087B CN202210069955.7A CN202210069955A CN114088087B CN 114088087 B CN114088087 B CN 114088087B CN 202210069955 A CN202210069955 A CN 202210069955A CN 114088087 B CN114088087 B CN 114088087B
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.)
Active
Application number
CN202210069955.7A
Other languages
English (en)
Other versions
CN114088087A (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

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 100002_DEST_PATH_IMAGE002
；
其中，
Figure 100002_DEST_PATH_IMAGE004
表示IMU坐标系下的第k个关键帧时刻，
Figure 100002_DEST_PATH_IMAGE006
表示
Figure DEST_PATH_IMAGE007
到
Figure DEST_PATH_IMAGE009
帧时刻之间位置的变化量，
Figure DEST_PATH_IMAGE011
表示
Figure DEST_PATH_IMAGE007A
到
Figure DEST_PATH_IMAGE009A
帧时刻之间速度的变化量，
Figure 100002_DEST_PATH_IMAGE013
表示
Figure DEST_PATH_IMAGE014
到
Figure DEST_PATH_IMAGE009AA
帧时刻之间姿态的变化量，
Figure DEST_PATH_IMAGE016
表示世界坐标系到
Figure DEST_PATH_IMAGE007AA
帧时刻IMU坐标系的旋转，
Figure DEST_PATH_IMAGE018
表示
Figure DEST_PATH_IMAGE009AAA
帧时刻在世界坐标系的位置，
Figure DEST_PATH_IMAGE020
表示
Figure DEST_PATH_IMAGE007AAA
帧时刻在世界坐标系的位置，
Figure DEST_PATH_IMAGE022
表示世界坐标系下重力加速度，
Figure 100002_DEST_PATH_IMAGE024
表示
Figure DEST_PATH_IMAGE007AAAA
帧时刻到
Figure DEST_PATH_IMAGE025
帧时刻的时间间隔，
Figure DEST_PATH_IMAGE027
表示
Figure DEST_PATH_IMAGE007_5A
帧时刻在世界坐标系下的速度，
Figure DEST_PATH_IMAGE029
表示
Figure DEST_PATH_IMAGE025A
帧时刻在世界坐标系下的速度，
Figure DEST_PATH_IMAGE031
表示
Figure DEST_PATH_IMAGE007_6A
帧时刻在世界坐标系下的姿态，
Figure DEST_PATH_IMAGE033
表示
Figure DEST_PATH_IMAGE009AAAA
帧时刻在世界坐标系下的姿态，
Figure DEST_PATH_IMAGE035
表示
Figure DEST_PATH_IMAGE009_5A
帧时刻IMU加速度计的偏置，
Figure DEST_PATH_IMAGE037
表示
Figure DEST_PATH_IMAGE007_7A
帧时刻IMU加速度计的偏置，
Figure DEST_PATH_IMAGE039
表示
Figure DEST_PATH_IMAGE009_6A
帧时刻IMU陀螺仪的偏置，
Figure DEST_PATH_IMAGE041
表示
Figure DEST_PATH_IMAGE007_8A
帧时刻IMU陀螺仪的偏置。
可选地，所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其中，所述无人机GPS-DENIED下高可靠高精度导航定位方法还包括：
获取滑动窗口中的状态变量，所述状态变量包括：滑动窗口内的n+1个关键帧时刻的惯性传感器坐标系的位置、速度、姿态、加速度计偏置、陀螺仪偏置、双目相机到惯性传感器的外参和m+1个 3D 路标点的逆深度；
其中，待求解的状态变量表示公式如下：
Figure DEST_PATH_IMAGE043
；
其中，
Figure DEST_PATH_IMAGE045
表示要优化的n+1个关键帧时刻的状态量和m+1个路标点的集合，
Figure DEST_PATH_IMAGE047
表示
Figure DEST_PATH_IMAGE014A
帧时刻世界坐标系下的位置
Figure DEST_PATH_IMAGE049
、速度
Figure DEST_PATH_IMAGE051
、姿态
Figure DEST_PATH_IMAGE053
、IMU加速度计的偏置
Figure DEST_PATH_IMAGE055
以及IMU陀螺仪的偏置
Figure DEST_PATH_IMAGE057
的集合，
Figure DEST_PATH_IMAGE059
表示相机坐标系到IMU坐标系的平移
Figure DEST_PATH_IMAGE061
和旋转
Figure DEST_PATH_IMAGE063
。
可选地，所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其中，图优化代价函数公式如下：
Figure DEST_PATH_IMAGE065
Figure DEST_PATH_IMAGE067
；
其中，
Figure DEST_PATH_IMAGE069
表示边缘化的先验误差，
Figure DEST_PATH_IMAGE071
表示海塞矩阵，
Figure DEST_PATH_IMAGE073
表示IMU测量值的集合，
Figure DEST_PATH_IMAGE075
表示IMU在
Figure DEST_PATH_IMAGE007_9A
到
Figure DEST_PATH_IMAGE009_7A
帧之间的测量值，
Figure DEST_PATH_IMAGE077
表示
Figure DEST_PATH_IMAGE007_10A
到
Figure DEST_PATH_IMAGE009_8A
帧的位置，
Figure DEST_PATH_IMAGE079
表示预积分的误差，
Figure DEST_PATH_IMAGE081
表示点特征的集合，
Figure 100002_DEST_PATH_IMAGE083
表示第
Figure 100002_DEST_PATH_IMAGE085
张图片中第
Figure 100002_DEST_PATH_IMAGE087
个点特征的观测值，
Figure 100002_DEST_PATH_IMAGE089
表示第
Figure DEST_PATH_IMAGE085A
张图片中的第
Figure DEST_PATH_IMAGE087A
个点特征的位置，
Figure 100002_DEST_PATH_IMAGE091
表示点特征的重投影误差，
Figure 100002_DEST_PATH_IMAGE093
表示线特征的集合，
Figure 100002_DEST_PATH_IMAGE095
表示第
Figure 100002_DEST_PATH_IMAGE097
张图片中第
Figure 100002_DEST_PATH_IMAGE099
个点特征的观测值，
Figure 100002_DEST_PATH_IMAGE101
表示第
Figure DEST_PATH_IMAGE097A
张图片中的第
Figure 100002_DEST_PATH_IMAGE103
个线特征的位置，
Figure 100002_DEST_PATH_IMAGE105
表示线特征的重投影误差。
可选地，所述的无人机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 DEST_PATH_IMAGE002A
；（公式1）
其中，
Figure 100002_DEST_PATH_IMAGE004A
表示IMU坐标系下的第k个关键帧时刻，
Figure 100002_DEST_PATH_IMAGE107
表示
Figure DEST_PATH_IMAGE007_11A
到
Figure DEST_PATH_IMAGE009_9A
帧时刻之间位置的变化量，
Figure 100002_DEST_PATH_IMAGE109
表示
Figure DEST_PATH_IMAGE007_12A
到
Figure DEST_PATH_IMAGE009_10A
帧时刻之间速度的变化量，
Figure DEST_PATH_IMAGE013A
表示
Figure DEST_PATH_IMAGE014AA
到
Figure DEST_PATH_IMAGE110
帧时刻之间姿态的变化量，
Figure DEST_PATH_IMAGE112
表示世界坐标系到
Figure DEST_PATH_IMAGE007_13A
帧时刻IMU坐标系的旋转，
Figure DEST_PATH_IMAGE114
表示
Figure DEST_PATH_IMAGE009_11A
帧时刻在世界坐标系的位置，
Figure DEST_PATH_IMAGE116
表示
Figure DEST_PATH_IMAGE007_14A
帧时刻在世界坐标系的位置，
Figure DEST_PATH_IMAGE022A
表示世界坐标系下重力加速度，
Figure 100002_DEST_PATH_IMAGE024A
表示
Figure DEST_PATH_IMAGE007_15A
帧时刻到
Figure DEST_PATH_IMAGE025AA
帧时刻的时间间隔，
Figure DEST_PATH_IMAGE118
表示
Figure DEST_PATH_IMAGE007_16A
帧时刻在世界坐标系下的速度，
Figure DEST_PATH_IMAGE120
表示
Figure DEST_PATH_IMAGE025AAA
帧时刻在世界坐标系下的速度，
Figure DEST_PATH_IMAGE122
表示
Figure DEST_PATH_IMAGE007_17A
帧时刻在世界坐标系下的姿态，
Figure DEST_PATH_IMAGE124
表示
Figure DEST_PATH_IMAGE009_12A
帧时刻在世界坐标系下的姿态，
Figure DEST_PATH_IMAGE126
表示
Figure DEST_PATH_IMAGE009_13A
帧时刻IMU加速度计的偏置，
Figure DEST_PATH_IMAGE128
表示
Figure DEST_PATH_IMAGE007_18A
帧时刻IMU加速度计的偏置，
Figure DEST_PATH_IMAGE130
表示
Figure DEST_PATH_IMAGE009_14A
帧时刻IMU陀螺仪的偏置，
Figure DEST_PATH_IMAGE132
表示
Figure DEST_PATH_IMAGE007_19A
帧时刻IMU陀螺仪的偏置。
步骤S20、初始化所述双目相机，根据所述双目相机的视差和基线计算相机位姿和路标点的深度。
具体地，首先进行双目相机的初始化，双目相机存在视差，视差就是从有一定距离的两个点上即两个摄像头，观察同一个目标所产生的方向差异，从目标看两个点之间的夹角，叫做这两个点的视差角，两点之间的连线称作基线。只要知道视差角度和基线长度，就可以计算出目标和观测者之间的距离。根据双目相机的视差和基线计算出相机位姿和路标点即图像特征的深度。进而可以利用计算出的相机的位姿来进行无人机的导航，路标点的深度来对环境进行建图。因为陀螺仪也可以积分出相机的旋转，所以可以通过相机计算出来的旋转等于IMU积分出来的旋转，这一等式计算出IMU的偏差，然后通过计算出来的偏差对先前的IMU进行重新积分，再通过将IMU和相机计算出来的位姿进行一步优化。最终能够得到一个无人机当前时刻不太准确的位姿，作为后端非线性优化的初值。
本发明的后端采用了滑动窗口的形式， 将点特征和线特征一起进行优化，滑动窗口中的状态变量包括滑动窗口内的n+1个关键帧时刻 IMU 坐标系的位置、速度、姿态（旋转）、加速度计偏置、陀螺仪偏置， Camera 到 IMU 的外参， m +1个 3D 路标点的逆深度，待求解的状态变量表示如公式2所示：
Figure DEST_PATH_IMAGE043A
；（公式2）
其中，
Figure DEST_PATH_IMAGE045A
表示要优化的n+1个关键帧时刻的状态量和m+1个路标点的集合，
Figure DEST_PATH_IMAGE047A
表示
Figure DEST_PATH_IMAGE014AAA
帧时刻世界坐标系下的位置
Figure DEST_PATH_IMAGE049A
、速度
Figure DEST_PATH_IMAGE134
、姿态
Figure DEST_PATH_IMAGE136
、IMU加速度计的偏置
Figure DEST_PATH_IMAGE055A
以及IMU陀螺仪的偏置
Figure DEST_PATH_IMAGE057A
的集合，
Figure DEST_PATH_IMAGE059A
表示相机坐标系到IMU坐标系的平移
Figure DEST_PATH_IMAGE061A
和旋转
Figure DEST_PATH_IMAGE063A
。
图优化代价函数如公式3所示：
Figure DEST_PATH_IMAGE065A
Figure DEST_PATH_IMAGE067A
；（公式3）
其中，
Figure DEST_PATH_IMAGE069A
表示边缘化的先验误差，
Figure DEST_PATH_IMAGE071A
表示海塞矩阵，
Figure DEST_PATH_IMAGE073A
表示IMU测量值的集合，
Figure DEST_PATH_IMAGE075A
表示IMU在
Figure DEST_PATH_IMAGE007_20A
到
Figure DEST_PATH_IMAGE009_15A
帧之间的测量值，
Figure DEST_PATH_IMAGE077A
表示
Figure DEST_PATH_IMAGE007_21A
到
Figure DEST_PATH_IMAGE009_16A
帧的位置，
Figure DEST_PATH_IMAGE079A
表示预积分的误差，
Figure DEST_PATH_IMAGE081A
表示点特征的集合，
Figure DEST_PATH_IMAGE083A
表示第
Figure DEST_PATH_IMAGE085AA
张图片中第
Figure DEST_PATH_IMAGE087AA
个点特征的观测值，
Figure DEST_PATH_IMAGE138
表示第
Figure DEST_PATH_IMAGE085AAA
张图片中的第
Figure DEST_PATH_IMAGE087AAA
个点特征的位置，
Figure 100002_DEST_PATH_IMAGE091A
表示点特征的重投影误差，
Figure 100002_DEST_PATH_IMAGE093A
表示线特征的集合，
Figure DEST_PATH_IMAGE140
表示第
Figure DEST_PATH_IMAGE097AA
张图片中第
Figure DEST_PATH_IMAGE099A
个点特征的观测值，
Figure DEST_PATH_IMAGE142
表示第
Figure DEST_PATH_IMAGE097AAA
张图片中的第
Figure 100002_DEST_PATH_IMAGE103A
个线特征的位置，
Figure DEST_PATH_IMAGE144
表示线特征的重投影误差。图优化的结构表示如图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 DEST_PATH_IMAGE146
，但是实际上陀螺仪的输出是一条复合白噪声信号缓慢变化的曲线，曲线的平均值就是零偏值。因为零偏的存在，估计的位姿误差会越来越大。因此，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 DEST_PATH_IMAGE002
；
其中，
Figure DEST_PATH_IMAGE004
表示IMU坐标系下的第k个关键帧时刻，
Figure DEST_PATH_IMAGE006
表示
Figure DEST_PATH_IMAGE004A
到
Figure DEST_PATH_IMAGE008
帧时刻之间位置的变化量，
Figure DEST_PATH_IMAGE010
表示
Figure DEST_PATH_IMAGE004AA
到
Figure DEST_PATH_IMAGE008A
帧时刻之间速度的变化量，
Figure DEST_PATH_IMAGE012
表示
Figure DEST_PATH_IMAGE013
到
Figure DEST_PATH_IMAGE008AA
帧时刻之间姿态的变化量，
Figure DEST_PATH_IMAGE015
表示世界坐标系到
Figure DEST_PATH_IMAGE004AAA
帧时刻IMU坐标系的旋转，
Figure DEST_PATH_IMAGE017
表示
Figure DEST_PATH_IMAGE008AAA
帧时刻在世界坐标系的位置，
Figure DEST_PATH_IMAGE019
表示
Figure DEST_PATH_IMAGE004AAAA
帧时刻在世界坐标系的位置，
Figure DEST_PATH_IMAGE021
表示世界坐标系下重力加速度，
Figure DEST_PATH_IMAGE023
表示
Figure DEST_PATH_IMAGE004_5A
帧时刻到
Figure DEST_PATH_IMAGE024
帧时刻的时间间隔，
Figure DEST_PATH_IMAGE026
表示
Figure DEST_PATH_IMAGE004_6A
帧时刻在世界坐标系下的速度，
Figure DEST_PATH_IMAGE028
表示
Figure DEST_PATH_IMAGE008AAAA
帧时刻在世界坐标系下的速度，
Figure DEST_PATH_IMAGE030
表示
Figure DEST_PATH_IMAGE032
帧时刻在世界坐标系下的姿态，
Figure DEST_PATH_IMAGE034
表示
Figure DEST_PATH_IMAGE008_5A
帧时刻在世界坐标系下的姿态，
Figure DEST_PATH_IMAGE036
表示
Figure DEST_PATH_IMAGE008_6A
帧时刻IMU加速度计的偏置，
Figure DEST_PATH_IMAGE038
表示
Figure DEST_PATH_IMAGE004_7A
帧时刻IMU加速度计的偏置，
Figure DEST_PATH_IMAGE040
表示
Figure DEST_PATH_IMAGE024A
帧时刻IMU陀螺仪的偏置，
Figure DEST_PATH_IMAGE042
表示
Figure DEST_PATH_IMAGE004_8A
帧时刻IMU陀螺仪的偏置。
3.根据权利要求2所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其特征在于，所述无人机GPS-DENIED下高可靠高精度导航定位方法还包括：
获取滑动窗口中的状态变量，所述状态变量包括：滑动窗口内的n+1个关键帧时刻的惯性传感器坐标系的位置、速度、姿态、加速度计偏置、陀螺仪偏置、双目相机到惯性传感器的外参和m+1个 3D 路标点的逆深度；
其中，待求解的状态变量表示公式如下：
Figure DEST_PATH_IMAGE044
；
其中，
Figure DEST_PATH_IMAGE046
表示要优化的n+1个关键帧时刻的状态量和m+1个路标点的集合，
Figure DEST_PATH_IMAGE048
表示
Figure DEST_PATH_IMAGE050
帧时刻世界坐标系下的位置
Figure DEST_PATH_IMAGE052
、速度
Figure DEST_PATH_IMAGE054
、姿态
Figure DEST_PATH_IMAGE056
、IMU加速度计的偏置
Figure DEST_PATH_IMAGE058
以及IMU陀螺仪的偏置
Figure DEST_PATH_IMAGE060
的集合，
Figure DEST_PATH_IMAGE062
表示相机坐标系到IMU坐标系的平移
Figure DEST_PATH_IMAGE064
和旋转
Figure DEST_PATH_IMAGE066
。
4.根据权利要求3所述的无人机GPS-DENIED下高可靠高精度导航定位方法，其特征在于，图优化代价函数公式如下：
Figure DEST_PATH_IMAGE068
Figure DEST_PATH_IMAGE070
；
其中，
Figure DEST_PATH_IMAGE072
表示边缘化的先验误差，
Figure DEST_PATH_IMAGE074
表示海塞矩阵，
Figure DEST_PATH_IMAGE076
表示IMU测量值的集合，
Figure DEST_PATH_IMAGE078
表示IMU在
Figure DEST_PATH_IMAGE050A
到
Figure DEST_PATH_IMAGE080
帧之间的测量值，
Figure DEST_PATH_IMAGE082
表示
Figure DEST_PATH_IMAGE050AA
到
Figure DEST_PATH_IMAGE083
帧的位置，
Figure DEST_PATH_IMAGE085
表示预积分的误差，
Figure DEST_PATH_IMAGE087
表示点特征的集合，
Figure DEST_PATH_IMAGE089
表示第
Figure DEST_PATH_IMAGE091
张图片中第
Figure DEST_PATH_IMAGE093
个点特征的观测值，
Figure DEST_PATH_IMAGE095
表示第
Figure DEST_PATH_IMAGE091A
张图片中的第
Figure DEST_PATH_IMAGE093A
个点特征的位置，
Figure DEST_PATH_IMAGE097
表示点特征的重投影误差，
Figure DEST_PATH_IMAGE099
表示线特征的集合，
Figure DEST_PATH_IMAGE101
表示第
Figure DEST_PATH_IMAGE103
张图片中第
Figure DEST_PATH_IMAGE105
个点特征的观测值，
Figure DEST_PATH_IMAGE107
表示第
Figure DEST_PATH_IMAGE103A
张图片中的第
Figure DEST_PATH_IMAGE109
个线特征的位置，
Figure DEST_PATH_IMAGE111
表示线特征的重投影误差。
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 CN114088087A (zh) 2022-02-25
CN114088087B true 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)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114088087B (zh) * 2022-01-21 2022-04-15 深圳大学 无人机gps-denied下高可靠高精度导航定位方法和系统
CN117148871B (zh) * 2023-11-01 2024-02-27 中国民航管理干部学院 一种多无人机协同电力巡检方法及系统
CN117437563B (zh) * 2023-12-13 2024-03-15 黑龙江惠达科技股份有限公司 一种基于双目视觉的植保无人机打点方法、装置及设备
CN117739996B (zh) * 2024-02-21 2024-04-30 西北工业大学 一种基于事件相机惯性紧耦合的自主定位方法

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107909614A (zh) * 2017-11-13 2018-04-13 中国矿业大学 一种gps失效环境下巡检机器人定位方法
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、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 浙江大学 一种基于视觉与激光雷达的同时定位与地图构建方法
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 (6)

* 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辅助北斗的机器人定位算法
CN110487301B (zh) * 2019-09-18 2021-07-06 哈尔滨工程大学 一种雷达辅助机载捷联惯性导航系统初始对准方法
CN113625774B (zh) * 2021-09-10 2023-07-21 天津大学 局部地图匹配与端到端测距多无人机协同定位系统和方法
CN114088087B (zh) * 2022-01-21 2022-04-15 深圳大学 无人机gps-denied下高可靠高精度导航定位方法和系统

Patent Citations (12)

* 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 浙江大学 一种基于视觉与激光雷达的同时定位与地图构建方法
CN112634451A (zh) * 2021-01-11 2021-04-09 福州大学 一种融合多传感器的室外大场景三维建图方法
CN112802196A (zh) * 2021-02-01 2021-05-14 北京理工大学 基于点线特征融合的双目惯性同时定位与地图构建方法
CN113192140A (zh) * 2021-05-25 2021-07-30 华中科技大学 一种基于点线特征的双目视觉惯性定位方法和系统

Also Published As

Publication number Publication date
WO2023138007A1 (zh) 2023-07-27
CN114088087A (zh) 2022-02-25

Similar Documents

Publication Publication Date Title
CN114088087B (zh) 无人机gps-denied下高可靠高精度导航定位方法和系统
CN109084732B (zh) 定位与导航方法、装置及处理设备
CN109887057B (zh) 生成高精度地图的方法和装置
CN111156998B (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
CN112634451B (zh) 一种融合多传感器的室外大场景三维建图方法
CN109506642B (zh) 一种机器人多相机视觉惯性实时定位方法及装置
CN109885080B (zh) 自主控制系统及自主控制方法
CN110044354A (zh) 一种双目视觉室内定位与建图方法及装置
CN109520497A (zh) 基于视觉和imu的无人机自主定位方法
CN107909614B (zh) 一种gps失效环境下巡检机器人定位方法
CN112116651B (zh) 一种基于无人机单目视觉的地面目标定位方法和系统
CN112136137A (zh) 一种参数优化方法、装置及控制设备、飞行器
Hinzmann et al. Flexible stereo: constrained, non-rigid, wide-baseline stereo vision for fixed-wing aerial platforms
CN116007609A (zh) 一种多光谱图像和惯导融合的定位方法和计算系统
CN114018254B (zh) 一种激光雷达与旋转惯导一体化构架与信息融合的slam方法
CN116989772B (zh) 一种空地多模态多智能体协同定位与建图方法
CN105807083A (zh) 一种无人飞行器实时测速方法及系统
Ling et al. RGB-D inertial odometry for indoor robot via keyframe-based nonlinear optimization
CN116659490A (zh) 低成本视觉-惯性融合的slam方法
Cao et al. Visual-Inertial-Laser SLAM Based on ORB-SLAM3
CN114821363A (zh) 一种基于语义信息匹配的无人机定位建图方法及系统
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
Zhao et al. 2D monocular visual odometry using mobile-phone sensors
CN117268404B (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