CN112230242B - 位姿估计***和方法 - Google Patents
位姿估计***和方法 Download PDFInfo
- Publication number
- CN112230242B CN112230242B CN202011056176.0A CN202011056176A CN112230242B CN 112230242 B CN112230242 B CN 112230242B CN 202011056176 A CN202011056176 A CN 202011056176A CN 112230242 B CN112230242 B CN 112230242B
- Authority
- CN
- China
- Prior art keywords
- pose
- information
- pose information
- filter
- laser radar
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 29
- 230000000007 visual effect Effects 0.000 claims abstract description 36
- 230000001133 acceleration Effects 0.000 claims abstract description 23
- 239000011159 matrix material Substances 0.000 claims description 26
- 230000010354 integration Effects 0.000 claims description 21
- 238000004590 computer program Methods 0.000 claims description 12
- 238000012937 correction Methods 0.000 claims description 7
- 230000003287 optical effect Effects 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 238000010586 diagram Methods 0.000 description 11
- 238000005457 optimization Methods 0.000 description 11
- 230000006870 function Effects 0.000 description 8
- 238000010168 coupling process Methods 0.000 description 7
- 238000005859 coupling reaction Methods 0.000 description 6
- 230000008878 coupling Effects 0.000 description 5
- 238000013507 mapping Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 4
- 238000003491 array Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
Images
Classifications
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- 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/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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明提供了一种位姿估计***和方法，其中，该***包括：视觉惯性模块，视觉惯性模块用于获取目标物当前所处环境的图像信息和相应的三轴加速度和三轴角速度信息，并根据图像信息以及相应的三轴加速度和三轴角速度信息获取目标物的第一位姿信息；激光SLAM模块，激光SLAM模块用于通过激光雷达获取当前所处环境的点云数据，并根据点云数据获取激光雷达的位姿信息；EKF模块，EKF模块用于根据激光雷达的位姿信息对第一位姿信息进行修正，以获取目标物的第二位姿信息。根据本发明的位姿估计***，采用激光、视觉和IMU结合的方式，能够准确地获取目标位的位姿信息，从而大大提高了定位的鲁棒性和准确性。
Description
技术领域
本发明涉及即时定位与建图技术领域，具体涉及一种位姿估计***和一种位姿估计方法。
背景技术
SLAM(Simultaneous Localization and Mapping，即时定位与建图)是一种目标物(例如，机器人)在未知环境中估计自身运动并且建立周围环境地图的技术，它在无人机、自动驾驶、移动机器人导航、虚拟现实和增强现实等领域均有着广泛的应用。
相关技术中，一般是采用激光雷达SLAM或者视觉SLAM进行定位，然而，采用激光雷达SLAM进行定位时，激光雷达具有稀疏性，在空旷场景下不易定位；采用视觉SLAM进行定位时，容易受到光照条件影响，也就是说，采用上述两种方式进行定位时，鲁棒性和准确性均较低。
发明内容
本发明为解决上述技术问题，提供了一种位姿估计***，采用激光、视觉和IMU结合的方式，能够准确地获取目标位的位姿信息，从而大大提高了定位的鲁棒性和准确性。
本发明采用的技术方案如下：
一种位姿估计***，包括：视觉惯性模块，视觉惯性模块用于获取目标物当前所处环境的图像信息以及相应的三轴加速度和三轴角速度信息，并根据图像信息以及相应的三轴加速度和三轴角速度信息获取目标物的第一位姿信息；激光SLAM模块，激光SLAM模块用于通过激光雷达获取当前所处环境的点云数据，并根据点云数据获取激光雷达的位姿信息；EKF模块，EKF模块用于根据激光雷达的位姿信息对第一位姿信息进行修正，以获取目标物的第二位姿信息。
所述视觉惯性模块包括：视觉跟踪单元，所述视觉跟踪单元用于从所述图像信息中选取两帧目标图像，并提取所述两帧目标图像的特征点，以及通过KLT稀疏光流算法对所述两帧目标图像的特征点进行跟踪，以获取所述两帧目标图像的第一相对位姿，并获取相应的视觉重投影误差；IMU预积分单元，所述IMU预积分单元用于对所述两帧目标图像对应的三轴加速度和三轴角速度信息进行预积分处理，以获取所述两帧目标图像的第二相对位姿，并获取相应的IMU预积分残差；优化单元，所述优化单元用于根据所述视觉重投影误差和所述IMU预积分残差对所述第一相对位姿和所述第二相对位姿进行优化，以获取所述第一位姿信息。
所述EKF模块具体用于：根据所述激光雷达的位姿信息构建观测模型；根据所述激光雷达的位姿信息对应的置信度和协方差矩阵获取滤波器的卡尔曼增益；根据所述滤波器的卡尔曼增益对所述观测模型中所述滤波器的误差状态和所述协方差矩阵进行修正；根据修正后的所述滤波器的误差状态对所述第一位姿信息进行修正，以获取所述目标物的第二位姿信息。
所述EKF模块根据以下公式获取所述滤波器的卡尔曼增益：
其中，K为所述滤波器的卡尔曼增益；
为所述协方差矩阵；H为所述激光雷达的位姿信息；V为所述置信度。
所述EKF模块根据以下公式对所述滤波器的误差状态进行修正：
ΔX＝Kr，
其中，r为修正前的所述滤波器的误差状态；ΔX为修正后的所述滤波器的误差状态，K为所述滤波器的卡尔曼增益。
所述EKF模块根据以下公式对所述协方差矩阵进行修正：
其中，
为修正前的协方差矩阵；P为修正后的协方差矩阵；H为所述激光雷达的位姿信息；K为所述滤波器的卡尔曼增益；I为单位矩阵。
一种位姿估计方法，包括以下步骤：获取目标物当前所处环境的图像信息以及相应的三轴加速度和三轴角速度信息，并根据所述图像信息以及相应的三轴加速度和三轴角速度信息获取所述目标物的第一位姿信息；通过激光雷达获取当前所处环境的点云数据，并根据所述点云数据获取所述激光雷达的位姿信息；根据所述激光雷达的位姿信息对所述第一位姿信息进行修正，以获取所述目标物的第二位姿信息。
一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时，实现上述的位姿估计方法。
一种非临时性计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述的位姿估计方法。
一种计算机程序产品，当所述计算机程序产品中的指令由处理器执行时，执行上述的位姿估计方法。
本发明的有益效果：
本发明采用激光、视觉和IMU结合的方式，能够准确地获取目标位的位姿信息，从而大大提高了定位的鲁棒性和准确性。
附图说明
图1为本发明实施例的位姿估计***的方框示意图；
图2为本发明一个实施例的位姿估计***的方框示意图；
图3为本发明一个实施例的视觉与IMU进行基于滑动窗口图优化的紧耦合的逻辑示意图；
图4为本发明一个实施例的通过激光SLAM模块进行离线建图的逻辑示意图；
图5为本发明一个实施例的通过激光SLAM模块进行在线定位的逻辑示意图；
图6为本发明一个实施例的视觉惯性模块的输出与激光雷达通过EKF模块进行松耦合的逻辑示意图；
图7为本发明实施例的位姿估计方法的流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
图1是根据本发明实施例的位姿估计***的方框示意图。
如图1所示，本发明实施例的位姿估计***可包括：视觉惯性模块100、激光SLAM模块200和EKF(Extended Kalman Filter，扩展卡尔曼滤波器)模块300。
其中，视觉惯性模块100用于获取目标物当前所处环境的图像信息以及相应的三轴加速度和三轴角速度信息，并根据图像信息以及相应的三轴加速度和三轴角速度信息获取目标物的第一位姿信息，其中，目标物可为机器人；激光SLAM模块200用于通过激光雷达获取当前所处环境的点云数据，并根据点云数据获取激光雷达的位姿信息；EKF模块300用于根据激光雷达的位姿信息对第一位姿信息进行修正，以获取目标物的第二位姿信息。
具体而言，本发明实施例中，可先通过视觉惯性模块100根据获取到的图像信息和IMU(Inertial Measurement Unit，惯性测量单元)数据获取目标物的第一位姿信息，然后再根据激光SLAM模块200获取到的激光雷达的位姿信息对该第一位姿信息进行修正，以获取目标物的第二位姿信息。由此，本发明的定位方案采用激光、视觉和IMU结合的方式，能够继承多种传感器(激光、视觉、IMU)的优势，获取更加准确的机器人的位姿信息，从而大大提高了定位的鲁棒性和准确性。
下面结合具体实施例来详细说明如何通过视觉惯性模块100获取目标物的第一位姿信息。
根据本发明的一个实施例，如图2所示，视觉惯性模块100可包括：视觉跟踪单元110、IMU预积分单元120和优化单元130。
其中，视觉跟踪单元110用于从图像信息中选取两帧目标图像，并提取两帧目标图像的特征点，以及通过KLT稀疏光流算法对两帧目标图像的特征点进行跟踪，以获取两帧目标图像的第一相对位姿，并获取相应的视觉重投影误差；IMU预积分单元120用于对两帧目标图像对应的三轴加速度和三轴角速度信息进行预积分处理，以获取两帧目标图像的第二相对位姿，并获取相应的IMU预积分残差；优化单元130用于根据视觉重投影误差和IMU预积分残差对第一相对位姿和第二相对位姿进行优化，以获取第一位姿信息。
具体而言，如图3所示，在获取到目标物当前所处环境的图像信息(输入图像)以及相应的三轴加速度和三轴角速度信息(输入IMU数据)后，可先进行***初始化。其中，在***初始化的过程中，可通过视觉跟踪单元110实现视觉跟踪，即，可通过视觉跟踪单元110从图像信息中选取视差较大的两帧图像，并提取两帧目标图像的特征点，以及通过KLT稀疏光流算法对两帧目标图像的特征点进行跟踪，以获取两帧目标图像的第一相对位姿，并获取相应的视觉重投影误差，并通过IMU预积分单元120实现IMU预积分，即通过IMU预积分单元用于对所述两帧目标图像对应的IMU数据进行积分计算，以获取两帧目标图像的第二相对位姿，包括两帧目标图像的运动数据，例如，旋转、平移、速度等。
需要说明的是，在通过视觉跟踪单元110实现视觉跟踪，以及通过IMU预积分单元120实现IMU预积分时，需保证视觉与IMU预积分的结果对齐，其中，对齐两帧目标图像之间的相对旋转可以初始化出陀螺仪的偏置，对齐两帧目标图像之间的平移可以初始化出尺度因子。
进一步而言，可通过优化单元130实现后端联合优化，即采用基于非线性优化的方式优化前端计算的位姿(第一相对位姿和所述第二相对位姿)，以输出与IMU频率相同的位姿。
具体地，可采用基于滑动窗口图优化的方式，在一个滑动窗口内，联合最小化目标函数，以实现对第一相对位姿和所述第二相对位姿进行优化。其中，目标函数可由三个部分组成，即：视觉重投影误差、IMU预积分残差和边缘化残差，该目标函数可为：
其中，目标函数中的第一项为边缘化残差，第二项为IMU预积分残差，第三项为视觉重投影误差。
由此，本发明通过优化单元对视觉跟踪单元输出的第一相对位姿和IMU预积分单元输出的第二相对位姿进行优化，以实现将视觉与IMU进行基于滑动窗口图优化的紧耦合。
在实际应用过程中，如果仅通过紧耦合的方式进行定位，那么将大大增加计算量，为此，本发明实施例中，在将视觉与IMU进行基于滑动窗口图优化的紧耦合后，还需要将其输出结果与激光雷达通过EKF模块300进行松耦合，以实现效率与精度的折中。
下面结合具体实施例来详细说明如何将视觉惯性模块100的输出与激光雷达通过EKF模块300进行松耦合。
需要说明的是，在进行松耦合之前，还需要通过激光SLAM模块200获取激光雷达的位姿信息。
其中，在获取激光雷达的位姿信息之前，需要先通过激光SLAM模块200进行离线建图。
具体地，如图4所示，激光SLAM模块200可预先通过激光雷达采集场景中的点云数据，并通过NDT(Normal Distributions Transform，正太分布变换)配准根据输入点云数据求解激光雷达位姿和点云地图，然后，根据求解出的激光雷达位姿将激光雷达采集到的场景中的点云数据投影到世界坐标系下，并进行回环检测，以及在检测到回环时，通过回环约束进行全局优化，以优化激光雷达位姿和点云地图，从而输出离线地图。
在获取离线地图后，再通过激光SLAM模块200进行在线定位。
具体地，如图5所示，激光SLAM模块20可通过激光雷达获取当前所处环境的点云数据，在接收到当前所处环境的点云数据(输入激光点云)后，通过NDT可将当前所处环境的点云数据与离线地图进行配准，以获取激光雷达的位姿信息。
进一步而言，通过激光SLAM模块200获取激光雷达的位姿信息后，可将视觉惯性模块100输出的第一位姿信息作为滤波器的预测更新，并将激光SLAM模块200输出的激光雷达的位姿信息作为滤波器的观测更新，以实现将视觉惯性模块100的输出与激光雷达通过EKF模块300进行松耦合。
需要说明的是，在松耦合过程中，相机与激光雷达需做好硬件时间同步，即相机与激光雷达在同一时刻采样。
根据本发明的一个实施例，EKF模块300具体用于：根据激光雷达的位姿信息构建观测模型；根据激光雷达的位姿信息对应的置信度和协方差矩阵获取滤波器的卡尔曼增益；根据滤波器的卡尔曼增益对观测模型中滤波器的误差状态和协方差矩阵进行修正；根据修正后的滤波器的误差状态对第一位姿信息进行修正，以获取目标物的第二位姿信息。
具体而言，滤波器将***状态分为真实状态(true state)、标称状态(nominalstate)和误差状态(error state)。标称状态即无人车的理想值，无误差、噪声等；由于传感器存在一定的测量误差，真实状态，即标称状态加上误差状态，是一个实际的测量值或者估计值，这里真实状态指视觉惯性模块100输出的第一位姿信息，包括位置、速度、旋转、加速度偏置、陀螺仪偏置等。如图6所示，滤波器通过对误差状态进行建模和高斯估计，当接收到观测数据(激光雷达的位姿信息)时，可对滤波器的误差状态进行状态更新，并判断是否收敛，如果不收敛，则继续进行更新，如果收敛，则获取当前误差状态，并根据当前误差状态输出位姿。
具体地，首先可将激光雷达的位姿信息(位置和旋转)作为滤波器的观测，以根据激光雷达的位姿信息构建观测模型，其中，观测模型可通过以下公式表示：
r＝HX，
其中，X为观测真实状态，即激光雷达的位姿信息，其置信度为V，协方差矩阵为
r为滤波器的误差状态，H为观测真实状态与滤波器的误差状态的转换关系。
其次，可根据激光雷达的位姿信息X对应的置信度V和协方差矩阵
获取滤波器的卡尔曼增益K，其中，可根据以下公式获取滤波器的卡尔曼增益K：
其中，K为滤波器的卡尔曼增益；
为协方差矩阵；H为激光雷达的位姿信息；V为置信度。
然后，可根据滤波器的卡尔曼增益K对观测模型中滤波器的误差状态和协方差矩阵进行修正，其中，可根据以下公式对滤波器的误差状态进行修正：
ΔX＝Kr，
其中，r为修正前的滤波器的误差状态；ΔX为修正后的滤波器的误差状态，K为滤波器的卡尔曼增益。
可根据以下公式对协方差矩阵进行修正：
其中，
为修正前的协方差矩阵；P为修正后的协方差矩阵；H为激光雷达的位姿信息；K为滤波器的卡尔曼增益；I为单位矩阵。
最后，可根据修正后的所述滤波器的误差状态对所述第一位姿信息进行修正，以获取所述目标物的第二位姿信息。
其中，可将修正后的滤波器的误差状态与第一位姿信息进行叠加，以得到***需要估计的状态，即第二位姿信息。
综上所述，根据本发明实施例的位姿估计***，通过视觉惯性模块获取目标物当前所处环境的图像信息以及相应的三轴加速度和三轴角速度信息，并根据图像信息以及相应的三轴加速度和三轴角速度信息获取目标物的第一位姿信息，以及通过激光SLAM模块通过激光雷达获取当前所处环境的点云数据，并根据点云数据获取激光雷达的位姿信息，并通过EKF模块根据激光雷达的位姿信息对第一位姿信息进行修正，以获取目标物的第二位姿信息。由此，采用激光、视觉和IMU结合的方式，能够准确地获取目标位的位姿信息，从而大大提高了定位的鲁棒性和准确性。
对应上述实施例的位姿估计***，本发明还提出一种位姿估计方法。
如图7所示，本发明实施例的位姿估计方法可包括以下步骤：
S701，获取目标物当前所处环境的图像信息以及相应的三轴加速度和三轴角速度信息，并根据图像信息以及相应的三轴加速度和三轴角速度信息获取目标物的第一位姿信息。
S702，通过激光雷达获取当前所处环境的点云数据，并根据点云数据获取激光雷达的位姿信息。
S703，根据激光雷达的位姿信息对第一位姿信息进行修正，以获取目标物的第二位姿信息。
需要说明的是，本发明实施例的位姿估计方法更具体的实施方式可参照上述位姿估计***的实施例，在此不再赘述。
根据本发明实施例的位姿估计方法，获取目标物当前所处环境的图像信息以及相应的三轴加速度和三轴角速度信息，并根据图像信息以及相应的三轴加速度和三轴角速度信息获取目标物的第一位姿信息，以及通过激光雷达获取当前所处环境的点云数据，并根据点云数据获取激光雷达的位姿信息，以及根据激光雷达的位姿信息对第一位姿信息进行修正，以获取目标物的第二位姿信息。由此，采用激光、视觉和IMU结合的方式，能够准确地获取目标位的位姿信息，从而大大提高了定位的鲁棒性和准确性。
对应上述实施例，本发明还提出一种计算机设备。
本发明实施例的计算机设备包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时，实现上述实施例的位姿估计方法。
根据本发明实施例的计算机设备，采用激光、视觉和IMU结合的方式，能够准确地获取目标位的位姿信息，从而大大提高了定位的鲁棒性和准确性。
对应上述实施例，本发明还提出一种非临时性计算机可读存储介质。
本发明实施例的非临时性计算机可读存储介质上存储有计算机程序，该程序被处理器执行时实现上述的位姿估计方法。
根据本发明实施例的非临时性计算机可读存储介质，采用激光、视觉和IMU结合的方式，能够准确地获取目标位的位姿信息，从而大大提高了定位的鲁棒性和准确性。
对应上述实施例，本发明还提出一种计算机程序产品。
当计算机程序产品中的指令由处理器执行时，可执行上述实施例的位姿估计方法。
根据本发明实施例的计算机程序产品，采用激光、视觉和IMU结合的方式，能够准确地获取目标位的位姿信息，从而大大提高了定位的鲁棒性和准确性。
在本发明的描述中，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。“多个”的含义是两个或两个以上，除非另有明确具体的限定。
在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必针对相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行***、装置或设备(如基于计算机的***、包括处理器的***或其他可以从指令执行***、装置或设备取指令并执行指令的***)使用，或结合这些指令执行***、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行***、装置或设备或结合这些指令执行***、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行***执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。
此外，在本发明各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。
上述提到的存储介质可以是只读存储器，磁盘或光盘等。尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (8)
1.一种位姿估计***，其特征在于，包括：
视觉惯性模块，所述视觉惯性模块用于获取目标物当前所处环境的图像信息以及相应的三轴加速度和三轴角速度信息，并根据所述图像信息以及相应的三轴加速度和三轴角速度信息获取所述目标物的第一位姿信息；
激光SLAM模块，所述激光SLAM模块用于通过激光雷达获取当前所处环境的点云数据，并根据所述点云数据获取所述激光雷达的位姿信息；
EKF模块，所述EKF模块用于根据所述激光雷达的位姿信息对所述第一位姿信息进行修正，以获取所述目标物的第二位姿信息，所述EKF模块具体用于：根据所述激光雷达的位姿信息构建观测模型；根据所述激光雷达的位姿信息对应的置信度和协方差矩阵获取滤波器的卡尔曼增益；根据所述滤波器的卡尔曼增益对所述观测模型中所述滤波器的误差状态和所述协方差矩阵进行修正；根据修正后的所述滤波器的误差状态对所述第一位姿信息进行修正，以获取所述目标物的第二位姿信息。
2.根据权利要求1所述的位姿估计***，其特征在于，所述视觉惯性模块包括：
视觉跟踪单元，所述视觉跟踪单元用于从所述图像信息中选取两帧目标图像，并提取所述两帧目标图像的特征点，以及通过KLT稀疏光流算法对所述两帧目标图像的特征点进行跟踪，以获取所述两帧目标图像的第一相对位姿，并获取相应的视觉重投影误差；
IMU预积分单元，所述IMU预积分单元用于对所述两帧目标图像对应的三轴加速度和三轴角速度信息进行预积分处理，以获取所述两帧目标图像的第二相对位姿，并获取相应的IMU预积分残差；
优化单元，所述优化单元用于根据所述视觉重投影误差和所述IMU预积分残差对所述第一相对位姿和所述第二相对位姿进行优化，以获取所述第一位姿信息。
3.根据权利要求1所述的位姿估计***，其特征在于，所述EKF模块根据以下公式获取所述滤波器的卡尔曼增益：
其中，K为所述滤波器的卡尔曼增益；
为所述协方差矩阵；H为所述激光雷达的位姿信息；V为所述置信度。
4.根据权利要求1所述的位姿估计***，其特征在于，所述EKF模块根据以下公式对所述滤波器的误差状态进行修正：
ΔX＝Kr，
其中，r为修正前的所述滤波器的误差状态；ΔX为修正后的所述滤波器的误差状态，K为所述滤波器的卡尔曼增益。
5.根据权利要求1所述的位姿估计***，其特征在于，所述EKF模块根据以下公式对所述协方差矩阵进行修正：
其中，
为修正前的协方差矩阵；P为修正后的协方差矩阵；H为所述激光雷达的位姿信息；K为所述滤波器的卡尔曼增益；I为单位矩阵。
6.一种位姿估计方法，其特征在于，包括以下步骤：
获取目标物当前所处环境的图像信息以及相应的三轴加速度和三轴角速度信息，并根据所述图像信息以及相应的三轴加速度和三轴角速度信息获取所述目标物的第一位姿信息；
通过激光雷达获取当前所处环境的点云数据，并根据所述点云数据获取所述激光雷达的位姿信息；
根据所述激光雷达的位姿信息对所述第一位姿信息进行修正，以获取所述目标物的第二位姿信息，具体根据所述激光雷达的位姿信息构建观测模型；根据所述激光雷达的位姿信息对应的置信度和协方差矩阵获取滤波器的卡尔曼增益；根据所述滤波器的卡尔曼增益对所述观测模型中所述滤波器的误差状态和所述协方差矩阵进行修正；根据修正后的所述滤波器的误差状态对所述第一位姿信息进行修正，以获取所述目标物的第二位姿信息。
7.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时，实现根据权利要求6所述的位姿估计方法。
8.一种非临时性计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现根据权利要求6所述的位姿估计方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011056176.0A CN112230242B (zh) | 2020-09-30 | 2020-09-30 | 位姿估计***和方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011056176.0A CN112230242B (zh) | 2020-09-30 | 2020-09-30 | 位姿估计***和方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112230242A CN112230242A (zh) | 2021-01-15 |
| CN112230242B true CN112230242B (zh) | 2023-04-25 |
Family
ID=74119672
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011056176.0A Active CN112230242B (zh) | 2020-09-30 | 2020-09-30 | 位姿估计***和方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112230242B (zh) |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112964276B (zh) * | 2021-02-09 | 2022-08-05 | 中国科学院深圳先进技术研究院 | 一种基于激光和视觉融合的在线标定方法 |
| CN113219492A (zh) * | 2021-03-30 | 2021-08-06 | 苏州市卫航智能技术有限公司 | 一种河道船舶行驶定位导航的方法及*** |
| CN112948411B (zh) * | 2021-04-15 | 2022-10-18 | 深圳市慧鲤科技有限公司 | 位姿数据的处理方法及接口、装置、***、设备和介质 |
| CN113390422B (zh) * | 2021-06-10 | 2022-06-10 | 奇瑞汽车股份有限公司 | 汽车的定位方法、装置及计算机存储介质 |
| CN113587934B (zh) * | 2021-07-30 | 2024-03-19 | 深圳市普渡科技有限公司 | 一种机器人、室内定位方法、装置和可读存储介质 |
| CN113739819B (zh) * | 2021-08-05 | 2024-04-16 | 上海高仙自动化科技发展有限公司 | 校验方法、装置、电子设备、存储介质及芯片 |
| CN113933861B (zh) * | 2021-11-12 | 2022-06-07 | 成都航维智芯科技有限公司 | 机载激光雷达点云生成方法及*** |
| CN114413898B (zh) * | 2022-03-29 | 2022-07-29 | 深圳市边界智控科技有限公司 | 多传感器数据融合方法、装置、计算机设备及存储介质 |
| CN115342806A (zh) * | 2022-07-14 | 2022-11-15 | 歌尔股份有限公司 | 头戴显示设备的定位方法、装置、头戴显示设备及介质 |
| CN116958452A (zh) * | 2023-09-18 | 2023-10-27 | 北京格镭信息科技有限公司 | 三维重建方法和*** |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108536149A (zh) * | 2018-04-25 | 2018-09-14 | 吉林大学 | 一种基于Dubins路径的无人驾驶车辆避障控制装置及控制方法 |
| CN109710724A (zh) * | 2019-03-27 | 2019-05-03 | 深兰人工智能芯片研究院（江苏）有限公司 | 一种构建点云地图的方法和设备 |
| CN109828592A (zh) * | 2019-04-22 | 2019-05-31 | 深兰人工智能芯片研究院（江苏）有限公司 | 一种障碍物检测的方法及设备 |
| US10390003B1 (en) * | 2016-08-29 | 2019-08-20 | Perceptln Shenzhen Limited | Visual-inertial positional awareness for autonomous and non-autonomous device |
| CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
| CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
| CN110617814A (zh) * | 2019-09-26 | 2019-12-27 | 中国科学院电子学研究所 | 单目视觉和惯性传感器融合的远距离测距***及方法 |
| CN111354043A (zh) * | 2020-02-21 | 2020-06-30 | 集美大学 | 一种基于多传感器融合的三维姿态估计方法及装置 |
| CN111380515A (zh) * | 2018-12-29 | 2020-07-07 | 纳恩博（常州）科技有限公司 | 定位方法及装置、存储介质、电子装置 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018026544A1 (en) * | 2016-07-22 | 2018-02-08 | Regents Of The University Of Minnesota | Square-root multi-state constraint kalman filter for vision-aided inertial navigation system |
-
2020
- 2020-09-30 CN CN202011056176.0A patent/CN112230242B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10390003B1 (en) * | 2016-08-29 | 2019-08-20 | Perceptln Shenzhen Limited | Visual-inertial positional awareness for autonomous and non-autonomous device |
| CN108536149A (zh) * | 2018-04-25 | 2018-09-14 | 吉林大学 | 一种基于Dubins路径的无人驾驶车辆避障控制装置及控制方法 |
| CN111380515A (zh) * | 2018-12-29 | 2020-07-07 | 纳恩博（常州）科技有限公司 | 定位方法及装置、存储介质、电子装置 |
| CN109710724A (zh) * | 2019-03-27 | 2019-05-03 | 深兰人工智能芯片研究院（江苏）有限公司 | 一种构建点云地图的方法和设备 |
| CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
| CN109828592A (zh) * | 2019-04-22 | 2019-05-31 | 深兰人工智能芯片研究院（江苏）有限公司 | 一种障碍物检测的方法及设备 |
| CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
| CN110617814A (zh) * | 2019-09-26 | 2019-12-27 | 中国科学院电子学研究所 | 单目视觉和惯性传感器融合的远距离测距***及方法 |
| CN111354043A (zh) * | 2020-02-21 | 2020-06-30 | 集美大学 | 一种基于多传感器融合的三维姿态估计方法及装置 |
Non-Patent Citations (2)
| Title |
|---|
| "Loose Coupling Visual-lidar Odometry by combining VISO2 and LOAM";Min Yan et al.;《Proceedings of the 36th Chinese Control Conference》;20170728;6841-6846 * |
| "基于多传感器融合的移动机器人SLAM算法研究";周阳;《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》;20190915;17-46 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112230242A (zh) | 2021-01-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112230242B (zh) | 位姿估计***和方法 | |
| Dellenbach et al. | Ct-icp: Real-time elastic lidar odometry with loop closure | |
| US20200124421A1 (en) | Method and apparatus for estimating position | |
| CN107167826B (zh) | 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位***及方法 | |
| CN110178048B (zh) | 交通工具环境地图生成和更新的方法和*** | |
| JP6760114B2 (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
| EP2133662B1 (en) | Methods and system of navigation using terrain features | |
| Chien et al. | Visual odometry driven online calibration for monocular lidar-camera systems | |
| CN113406682B (zh) | 一种定位方法、装置、电子设备及存储介质 | |
| JP4984659B2 (ja) | 自車両位置推定装置 | |
| Sujiwo et al. | Monocular vision-based localization using ORB-SLAM with LIDAR-aided mapping in real-world robot challenge | |
| CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
| Jones et al. | Inertial structure from motion with autocalibration | |
| CN115407357A (zh) | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 | |
| Dawood et al. | Harris, SIFT and SURF features comparison for vehicle localization based on virtual 3D model and camera | |
| CN114693754B (zh) | 一种基于单目视觉惯导融合的无人机自主定位方法与*** | |
| CN108613675B (zh) | 低成本无人飞行器移动测量方法及*** | |
| CN115451948A (zh) | 一种基于多传感器融合的农业无人车定位里程计方法及*** | |
| CN115135963A (zh) | 用于在场景地图中生成3d参考点的方法 | |
| CN114396943A (zh) | 一种融合定位方法与终端 | |
| Khoshelham et al. | Vehicle positioning in the absence of GNSS signals: Potential of visual-inertial odometry | |
| CN109741372A (zh) | 一种基于双目视觉的里程计运动估计方法 | |
| WO2020113425A1 (en) | Systems and methods for constructing high-definition map | |
| CN116202511B (zh) | 长巷道超宽带一维约束下的移动装备位姿确定方法及装置 | |
| CN107270904B (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 | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20240516 Address after: Room 6227, No. 999, Changning District, Shanghai 200050 Patentee after: Shenlan robot (Shanghai) Co.,Ltd. Country or region after: China Address before: 518131 room 115, building 8, 1970 Science Park, Minzhi community, Minzhi street, Longhua District, Shenzhen City, Guangdong Province Patentee before: Shenlan artificial intelligence (Shenzhen) Co.,Ltd. Country or region before: China |