CN112815939A - 移动机器人的位姿估计方法及计算机可读存储介质 - Google Patents

移动机器人的位姿估计方法及计算机可读存储介质 Download PDF

Info

Publication number
CN112815939A
CN112815939A CN202110003763.1A CN202110003763A CN112815939A CN 112815939 A CN112815939 A CN 112815939A CN 202110003763 A CN202110003763 A CN 202110003763A CN 112815939 A CN112815939 A CN 112815939A
Authority
CN
China
Prior art keywords
mobile robot
coordinate system
time
pose estimation
data
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
CN202110003763.1A
Other languages
English (en)
Other versions
CN112815939B (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 International Graduate School of Tsinghua University
Original Assignee
Shenzhen International Graduate School of Tsinghua 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 International Graduate School of Tsinghua University filed Critical Shenzhen International Graduate School of Tsinghua University
Priority to CN202110003763.1A priority Critical patent/CN112815939B/zh
Publication of CN112815939A publication Critical patent/CN112815939A/zh
Application granted granted Critical
Publication of CN112815939B publication Critical patent/CN112815939B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供一种移动机器人的位姿估计方法及计算机可读存储介质，方法包括：获取多源传感器数据，多源传感器数据包括：图像采集设备采集地场景图像、惯性测量单元采集地数据、轮式编码器采集地数据；根据场景图像检测场景动态将所述移动机器人的位姿估计初始化；惯性测量单元采集地数据、轮式编码器采集地数据通过状态转移方程计算所述移动机器人当前时刻状态向量的先验估计量及对应的协方差传递矩阵；根据场景图像对当前时刻状态向量的先验估计量及对应的协方差矩阵迭代至收敛完成滤波更新。通过将图像采集设备采集数据、惯性测量单元、轮式编码器采集地数据三者共同作为位姿估计的输入数据三者结合，优势互补，可以得到紧耦合的位姿估计数据。

Description

移动机器人的位姿估计方法及计算机可读存储介质
技术领域
本发明涉及机器人定位技术领域，尤其涉及一种移动机器人的位姿估计方法及计算机可读存储介质。
背景技术
同时定位与地图构建(simultaneous localization and mapping，SLAM)是赋予机器人智能感知的关键技术之一，该技术能获得移动机器人本身的位置和周围环境的地图。经过几十年的研究发展，SLAM问题融合了概率估计、射影几何、信号处理、数值优化等多个领域的理论与研究成果，但究其本质，仍可视为由运动方程和观测方程构建的状态估计问题。SLAM的运动方程含义为从k-1时刻到k时刻，机器人的位置变化情况。数学上可表示为：
xk＝f(xk-1，μk，ωk)
其中，μk为传感器测量读数，ωk为噪声。
SLAM的观测方程含义为k时刻，机器人于位置xk处探测到了某一个路标yj，产生一个观测数据zk，j：
zk，j＝h(yj，xk，vk，j)
其中，vk，j为观测噪声。
故SLAM问题的研究的是，在知道运动测量的读数μ以及传感器读数z时，如何求解定位问题(估计x)与建图问题(估计y)。
目前SLAM主要有两种实现途径，基于激光传感器和基于视觉传感器。激光SLAM虽然可以实现更高的定位精度，但激光传感器的要求较高，也探测范围受到限制；而视觉SLAM对传感器要求较低，探测范围不受限制，因此更加适用于移动机器人的定位场景。但纯视觉对于运动模糊、遮挡、光照变化等条件十分敏感，严重影响定位精度，因此通过多源传感器融合的方式降低定位误差。
较为棘手的是，SLAM问题是一个非线性问题，在位姿估计的过程中，计算过程较为复杂。一般情况下，通过线性化的方式近似求解非线性问题，因此在求解精度上有一定牺牲；虽然采用无迹卡尔曼滤波通过拟合状态变量的概率密度分布代替将非线性问题线性化，可以提高求解精度，但使用的数据来源可能只有图像采集设备和IMU(InertialMeasurement Unit，惯性测量单元)或者雷达和图像采集设备的数据，精度依然不高。
现有技术中缺乏一种高精度的位姿估计方法。
以上背景技术内容的公开仅用于辅助理解本发明的构思及技术方案，其并不必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请的申请日已经公开的情况下，上述背景技术不应当用于评价本申请的新颖性和创造性。
发明内容
本发明为了解决现有的问题，提供一种移动机器人的位姿估计方法及计算机可读存储介质。
为了解决上述问题，本发明采用的技术方案如下所述：
一种移动机器人的位姿估计方法，包括如下步骤：S1：获取多源传感器数据，所述多源传感器数据包括：图像采集设备采集地场景图像、惯性测量单元采集地数据、轮式编码器采集地数据；S2：根据所述场景图像检测场景动态将所述移动机器人的位姿估计初始化；S3：所述惯性测量单元采集地数据、所述轮式编码器采集地数据通过状态转移方程计算所述移动机器人当前时刻状态向量的先验估计量及对应的协方差传递矩阵；S4：根据所述场景图像对所述当前时刻状态向量的先验估计量及对应的协方差矩阵迭代至收敛完成滤波更新。
优选地，还包括如下步骤：S5：通过滑动窗口优化校准得到具有一致性的所述移动机器人的位姿估计。
优选地，图像采集设备采集地场景图像包括当前时刻的图像帧；惯性测量单元采集地数据包括角速度和加速度；轮式编码器采集地数据包括所述移动机器人的左、右两轮的行进速度。
优选地，根据所述场景图像检测场景动态将所述移动机器人的位姿估计初始化包括：根据所述场景图像的连续图像帧进行角点检测估计匹配点对的运动状态进而确定所述移动机器人的状态；若所述移动机器人处于运动状态，则通过所述图像采集设备、所述惯性测量单元、所述轮式编码器进行状态估计完成所述移动机器人的位姿估计初始化；若所述移动机器人处于静止状态，则通过所述惯性测量单元完成所述移动机器人的位姿估计初始化。
优选地，所述惯性测量单元包括陀螺仪和加速度计，通过所述惯性测量单元完成所述移动机器人的位姿估计初始化包括如下步骤：将陀螺仪偏差bias bgk设定为所述陀螺仪的静态数据，加速度计偏差bias bak、bek设为0：
bgk＝mean(arrygyro)
bak＝0
bek＝0
其中，mean(arrygyro)表示陀螺仪静态测量数据的均值；
根据加速度计静态数据和重力向量计算从机体坐标系{Bk}到世界坐标系{W}的旋转四元数
Figure BDA0002882749760000031
gb＝mean(arrygyro)
gw＝(0 0 -9.8)T
Figure BDA0002882749760000032
其中，mean(arrygyro)表示加速度计静态测量数据的均值，gw表示世界坐标系{W}下的重力向量，运算符⊙表示向量做差后的四元数转化；
将所述状态向量中的其余量均设为0，完成初始化。
优选地，所述惯性测量单元采集地数据、所述轮式编码器采集地数据通过状态转移方程计算所述移动机器人当前时刻状态向量的先验估计量及对应的协方差传递矩阵包括如下步骤：S31：确定所述移动机器人当前时刻状态向量，所述当前时刻k的状态向量Xk由所述惯性测量单元及所述轮式编码器采集地所述移动机器人运动状态
Figure BDA0002882749760000033
和路标点的位置
Figure BDA0002882749760000034
两部分构成：
Figure BDA0002882749760000035
Figure BDA0002882749760000036
Figure BDA0002882749760000041
其中，
Figure BDA0002882749760000042
表示时刻k从机体坐标系{Bk}到世界坐标系{W}的旋转四元数，
Figure BDA0002882749760000043
和
Figure BDA0002882749760000044
表示以世界坐标系{W}为参考，机体坐标系{Bk}的3D位置与运动速度，
Figure BDA00028827497600000417
表示时刻k机体坐标系{Bk}到世界坐标系{W}的轮式编码器移动距离，bgk、bak、bek分别为陀螺仪、加速度计、轮式编码器偏差bias，
Figure BDA0002882749760000045
为世界坐标系{W}下路标点l的位置，ρ记为该路标点的逆深度；
S32：由所述状态向量根据运动学模型进行离散化处理得到离散时间下状态转移方程：
Figure BDA0002882749760000046
Figure BDA0002882749760000047
Figure BDA0002882749760000048
Figure BDA0002882749760000049
Figure BDA00028827497600000410
Figure BDA00028827497600000411
Figure BDA00028827497600000412
其中，
Figure BDA00028827497600000413
表示从时刻k-1到时刻k时，从机体坐标系{Bk}到世界坐标系{W}旋转四元数、3D位置、运动速度、移动距离的先验估计值，
Figure BDA00028827497600000414
分别为从时刻k-1到时刻k时惯性测量单元的陀螺仪、加速度计和轮式编码器的随机噪声先验估计值，
Figure BDA00028827497600000415
Figure BDA00028827497600000416
表示时刻k-1惯性测量单元中陀螺仪、加速度计的测量值，gW表示世界坐标系{W}下的重力向量，
Figure BDA00028827497600000418
为时刻k-1从机体坐标系{Bk}到世界坐标系{W}旋转矩阵估计值，Δt表示时间间隔；
S33：对所述状态向量进行Sigma点采样，并进行采样点的无迹变换：
Figure BDA0002882749760000051
其中，
Figure BDA0002882749760000059
为k-1时刻状态向量的第i个采样点，
Figure BDA00028827497600000510
为k-1时刻状态向量的均值，λ＝α2(n+κ)-n为决定Sigma点集缩放比例的参数，α代表Sigma点集到均值之间的距离，n为状态向量的维度，κ一般设为0或3-n，以保证n+κ＝3，而L为k-1时刻状态向量协方差矩阵Pk-1的Cholesky分解，即P＝L×LT；
所述Sigma点对应权值为：
Figure BDA0002882749760000052
Figure BDA0002882749760000053
Figure BDA0002882749760000054
Figure BDA0002882749760000055
其中，Wm为Sigma点均值的权值，WC为Sigma点协方差的权值；
S34：通过所述状态转移方程对所述状态向量进行时间更新，由k-1时刻的状态向量和协方差矩阵预测k时刻的状态向量先验估计和协方差矩阵：
Figure BDA0002882749760000056
Figure BDA0002882749760000057
Figure BDA0002882749760000058
其中，f(·)表示状态转移方程，为非线性函数；Qk表示k时刻下系统噪声方差矩阵。
优选地，根据所述场景图像对所述当前时刻状态向量的先验估计量及对应的协方差矩阵迭代至收敛完成滤波更新包括如下步骤：
S41：将采用逆深度模型来表征所述当前时刻状态向量中路标点的位置
Figure BDA0002882749760000061
Figure BDA0002882749760000062
其中，
Figure BDA0002882749760000063
为世界坐标系{W}下路标点l的位置，ρ记为该路标点的逆深度，y(·)为表征函数；
S42：根据所述路标点的位置通过反投影观测方程预测观测量
Figure BDA0002882749760000064
Figure BDA0002882749760000065
其中，
Figure BDA00028827497600000610
表示时刻k从机体坐标系{Bk}到世界坐标系{W}的旋转矩阵，
Figure BDA00028827497600000611
表示从图像采集设备坐标系{C}到机体坐标系{B}的旋转矩阵，σkl表示k时刻路标点l的观测噪声，π(·)表示由图像采集设备内参决定的投影函数。
S43：根据所述观测量进行无迹变换并计算所述观测量的均值和协方差矩阵：
Figure BDA0002882749760000066
Figure BDA0002882749760000067
Figure BDA0002882749760000068
S44：根据所述先验估计和所述观测量的均值后验估计得所述计算卡尔曼增益K、更新状态向量Xk和协方差矩阵Pk：
Kk＝Tk|k-1Sk|k-1 -1
Figure BDA0002882749760000069
Pk＝Pk|k-1-KkSk|k-1Kk T
S45：迭代循环上述步骤S42-S44，直至状态向量Xk和协方差矩阵Pk收敛。
优选地，通过滑动窗口优化校准得到具有一致性的所述移动机器人的位姿估计包括如下步骤：S51：确定待优化变量χ包含从时刻i到j的所述场景图像的连续关键帧图像和所述关键帧图像观测到的n个路标点：
Figure BDA0002882749760000071
其中，
Figure BDA0002882749760000075
表示时刻k对应的机体坐标系{Bk}下关键帧图像估计信息，
Figure BDA0002882749760000076
表示世界坐标系{W}下滑动窗口内第l个地图点的3D坐标；
由所述状态向量得到对应的误差状态向量：
Figure BDA0002882749760000072
S52：确定损失函数为如下形式：
Figure BDA0002882749760000073
其中，rp、
Figure BDA0002882749760000074
分别表示先验误差、惯性测量单元和轮速编码器测量值误差和重投影误差，∑p、∑Oij、∑Cij则为对应的协方差矩阵；
S53：求解所述损失函数。
优选地，采用Levenberg–Marquardt方法求解所述损失函数。
本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上任一所述方法的步骤。
本发明的有益效果为：提供一种移动机器人的位姿估计方法及计算机可读存储介质，通过将图像采集设备采集数据、惯性测量单元、轮式编码器采集地数据三者共同作为位姿估计的输入数据三者结合，优势互补，可以得到紧耦合的位姿估计数据。其中，图像采集设备采集数据包含丰富的环境特征信息，但受光照、运动模糊等条件变化影响；惯性测量单元IMU测量数据能提供自身旋转估计，但是测量过程受测量噪声影响较大；轮式编码器测量数据包含环境的绝对尺度信息，但缺少旋转方向信息。
进一步地，检测静态场景，提取连续图像帧中的角点并跟踪匹配，若检测到静态场景则单独使用IMU数据完成初始化过程，减少零速运动带来的漂移误差。
再进一步地，本发明采用无迹变换，通过拟合状态变量的概率密度分布替代非线性系统线性化，构建出的状态转移方程具有更高的模型精度，进而更好地提高位姿估计值的准确性。
更进一步地，采用滑动窗口处理关键帧序列，优化对象包括一系列关键帧及其观测到的路标点，通过求解最小二乘问题，得到优化后的状态量，以便维护建图结果的一致性。
附图说明
图1是本发明实施例中第一种移动机器人的位姿估计方法的示意图。
图2是本发明实施例中第一种移动机器人的位姿估计方法的示意图。
图3是本发明实施例中计算移动机器人当前时刻状态向量的先验估计量及对应的协方差传递矩阵的方法示意图。
图4是本发明实施例中根据所述场景图像对所述当前时刻状态向量的先验估计量及对应的协方差矩阵迭代至收敛完成滤波更新的方法示意图。
图5是本发明实施例中通过滑动窗口优化校准得到具有一致性的所述移动机器人的位姿估计方法示意图。
图6是本发明实施例中移动机器人的位姿估计系统的示意图。
图7是本发明实施例中飞行器的传感器装置安装示意图。
图8(a)-图8(d)分别是本发明实施例中数据集MH_01_easy、MH_02_easy、V1_02_medium和V2_03_difficult的估计轨迹与真实估计轨迹示意图。
图9(a)-图9(d)分别是本发明实施例中数据集MH_01_easy、MH_02_easy、V1_02_medium和V2_03_difficult的3D位置坐标的误差分析示意图。
具体实施方式
为了使本发明实施例所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
需要说明的是，当元件被称为“固定于”或“设置于”另一个元件，它可以直接在另一个元件上或者间接在该另一个元件上。当一个元件被称为是“连接于”另一个元件，它可以是直接连接到另一个元件或间接连接至该另一个元件上。另外，连接既可以是用于固定作用也可以是用于电路连通作用。
需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多该特征。在本发明实施例的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
如图1所示，一种移动机器人的位姿估计方法，其特征在于，包括如下步骤：
S1：获取多源传感器数据，所述多源传感器数据包括：图像采集设备采集地场景图像、惯性测量单元采集地数据、轮式编码器采集地数据；
S2：根据所述场景图像检测场景动态将所述移动机器人的位姿估计初始化；
S3：所述惯性测量单元采集地数据、所述轮式编码器采集地数据通过状态转移方程计算所述移动机器人当前时刻状态向量的先验估计量及对应的协方差传递矩阵；
S4：根据所述场景图像对所述当前时刻状态向量的先验估计量及对应的协方差矩阵迭代至收敛完成滤波更新。
本发明通过将图像采集设备采集数据、惯性测量单元、轮式编码器采集地数据三者共同作为位姿估计的输入数据三者结合，优势互补，可以得到紧耦合的位姿估计数据。其中，图像采集设备采集数据包含丰富的环境特征信息，但受光照、运动模糊等条件变化影响；惯性测量单元IMU测量数据能提供自身旋转估计，但是测量过程受测量噪声影响较大；轮式编码器测量数据包含环境的绝对尺度信息，但缺少旋转方向信息。
进一步地，检测静态场景，提取连续图像帧中的角点并跟踪匹配，若检测到静态场景则单独使用IMU数据完成初始化过程，减少零速运动带来的漂移误差。
再进一步地，本发明采用无迹变换，通过拟合状态变量的概率密度分布替代非线性系统线性化，构建出的状态转移方程具有更高的模型精度，进而更好地提高位姿估计值的准确性。
如图2所示，为了进一步提高精度，本发明还包括如下步骤：
S5：通过滑动窗口优化校准得到具有一致性的所述移动机器人的位姿估计。
本发明通过采用滑动窗口处理关键帧序列，优化对象包括一系列关键帧及其观测到的路标点，通过求解最小二乘问题，得到优化后的状态量，以便维护建图结果的一致性。
本发明所涉及的移动机器人包括数据采集的硬件包括：图像采集设备，一般为单目相机用于测量移动过程中的机器人“看见”的视频图形；惯性测量单元IMU用于测量移动过程中的角速度和线性加速度；以及轮式编码器用于测量移动机器人滚轮转动圈数。
在本发明的一种实施例中，图像采集设备采集地场景图像包括当前时刻的图像帧；
惯性测量单元采集地数据包括角速度和加速度；
轮式编码器采集地数据包括所述移动机器人的左、右两轮的行进速度。
可以理解的是，同时还需要获取当前时刻的图像帧对应的参考帧用于后续计算。
在本发明的一种实施例中，根据所述场景图像检测场景动态将所述移动机器人的位姿估计初始化包括：
根据所述场景图像的连续图像帧进行角点检测估计匹配点对的运动状态进而确定所述移动机器人的状态；
若所述移动机器人处于运动状态，则通过所述图像采集设备、所述惯性测量单元、所述轮式编码器进行状态估计完成所述移动机器人的位姿估计初始化；
若所述移动机器人处于静止状态，则通过所述惯性测量单元完成所述移动机器人的位姿估计初始化。
具体的，所述惯性测量单元包括陀螺仪和加速度计，通过所述惯性测量单元完成所述移动机器人的位姿估计初始化包括如下步骤：
将陀螺仪偏差bias bgk设定为所述陀螺仪的静态数据，加速度计偏差bias bak、bek设为0：
bgk＝mean(arrygyro)
bak＝0
bek＝0
其中，mean(arrygyro)表示陀螺仪静态测量数据的均值；
根据加速度计静态数据和重力向量计算从机体坐标系{Bk}到世界坐标系{W}的旋转四元数
Figure BDA0002882749760000111
gb＝mean(arrygyro)
gw＝(0 0 -9.8)T
Figure BDA0002882749760000112
其中，mean(arrygyro)表示加速度计静态测量数据的均值，gw表示世界坐标系{W}下的重力向量，运算符⊙表示向量做差后的四元数转化；
将所述状态向量中的其余量均设为0，完成初始化。可以理解的是，这里的其余向量包括
Figure BDA0002882749760000113
其中，
Figure BDA0002882749760000114
和
Figure BDA0002882749760000115
表示以世界坐标系{W}为参考，机体坐标系{Bk}的3D位置与运动速度，
Figure BDA00028827497600001110
表示时刻k机体坐标系{Bk}到世界坐标系{W}的轮式编码器移动距离，将其余向量均设为0，即：
Figure BDA0002882749760000116
Figure BDA0002882749760000117
Figure BDA0002882749760000118
如图3所示，所述惯性测量单元采集地数据、所述轮式编码器采集地数据通过状态转移方程计算所述移动机器人当前时刻状态向量的先验估计量及对应的协方差传递矩阵包括如下步骤：
S31：确定所述移动机器人当前时刻状态向量，所述当前时刻k的状态向量Xk由所述惯性测量单元及所述轮式编码器采集地所述移动机器人运动状态
Figure BDA0002882749760000119
和路标点的位置
Figure BDA0002882749760000121
两部分构成：
Figure BDA0002882749760000122
Figure BDA0002882749760000123
Figure BDA0002882749760000124
其中，
Figure BDA00028827497600001214
表示时刻k从机体坐标系{Bk}到世界坐标系{W}的旋转四元数，
Figure BDA00028827497600001215
和
Figure BDA00028827497600001216
表示以世界坐标系{W}为参考，机体坐标系{Bk}的3D位置与运动速度，
Figure BDA00028827497600001217
表示时刻k机体坐标系{Bk}到世界坐标系{W}的轮式编码器移动距离，bgk、bak、bek分别为陀螺仪、加速度计、轮式编码器偏差bias，
Figure BDA00028827497600001218
为世界坐标系{W}下路标点l的位置，ρ记为该路标点的逆深度；
S32：由所述状态向量根据运动学模型进行离散化处理得到离散时间下状态转移方程：
Figure BDA0002882749760000125
Figure BDA0002882749760000126
Figure BDA0002882749760000127
Figure BDA0002882749760000128
Figure BDA0002882749760000129
Figure BDA00028827497600001210
Figure BDA00028827497600001211
其中，
Figure BDA00028827497600001212
表示从时刻k-1到时刻k时，从机体坐标系{Bk}到世界坐标系{W}旋转四元数、3D位置、运动速度、移动距离的先验估计值，
Figure BDA00028827497600001213
分别为从时刻k-1到时刻k时惯性测量单元的陀螺仪、加速度计和轮式编码器的随机噪声先验估计值，
Figure BDA0002882749760000131
Figure BDA0002882749760000132
表示时刻k-1惯性测量单元中陀螺仪、加速度计的测量值，gW表示世界坐标系{W}下的重力向量，
Figure BDA00028827497600001310
为时刻k-1从机体坐标系{Bk}到世界坐标系{W}旋转矩阵估计值，Δt表示时间间隔；
S33：对所述状态向量进行Sigma点采样，并进行采样点的无迹变换：
Figure BDA0002882749760000133
其中，
Figure BDA00028827497600001311
为k-1时刻状态向量的第i个采样点，
Figure BDA00028827497600001312
为k-1时刻状态向量的均值，λ＝α2(n+k)-n为决定Sigma点集缩放比例的参数，α代表Sigma点集到均值之间的距离，n为状态向量的维度，κ一般设为0或3-n，以保证n+κ＝3，而L为k-1时刻状态向量协方差矩阵Pk-1的Cholesky分解，即P＝L×LT；
所述Sigma点对应权值为：
Figure BDA0002882749760000134
Figure BDA0002882749760000135
Figure BDA0002882749760000136
Figure BDA0002882749760000137
其中，Wm为Sigma点均值的权值，WC为Sigma点协方差的权值；
S34：通过所述状态转移方程对所述状态向量进行时间更新，由k-1时刻的状态向量和协方差矩阵预测k时刻的状态向量先验估计和协方差矩阵：
Figure BDA0002882749760000138
Figure BDA0002882749760000139
Figure BDA0002882749760000141
其中，f(·)表示状态转移方程，为非线性函数；Qk表示k时刻下系统噪声方差矩阵。
如图4所示，根据所述场景图像对所述当前时刻状态向量的先验估计量及对应的协方差矩阵迭代至收敛完成滤波更新包括如下步骤：
S41：将采用逆深度模型来表征所述当前时刻状态向量中路标点的位置
Figure BDA0002882749760000142
Figure BDA0002882749760000143
其中，
Figure BDA0002882749760000144
为世界坐标系{W}下路标点l的位置，ρ记为该路标点的逆深度，y(·)为表征函数；
S42：根据所述路标点的位置通过反投影观测方程预测观测量
Figure BDA0002882749760000145
Figure BDA0002882749760000146
其中，
Figure BDA00028827497600001410
表示时刻k从机体坐标系{Bk}到世界坐标系{W}的旋转矩阵，
Figure BDA00028827497600001411
表示从图像采集设备坐标系{C}到机体坐标系{B}的旋转矩阵，σkl表示k时刻路标点l的观测噪声，π(·)表示由图像采集设备内参决定的投影函数。
S43：根据所述观测量进行无迹变换并计算所述观测量的均值和协方差矩阵：
Figure BDA0002882749760000147
Figure BDA0002882749760000148
Figure BDA0002882749760000149
S44：根据所述先验估计和所述观测量的均值后验估计得所述计算卡尔曼增益K、更新状态向量Xk和协方差矩阵Pk：
Kk＝Tk|k-1Sk|k-1 -1
Figure BDA0002882749760000151
Pk＝Pk|k-1-KkSk|k-1Kk T
S45：迭代循环上述步骤S42-S44，直至状态向量Xk和协方差矩阵Pk收敛。
如图5所示，通过滑动窗口优化校准得到具有一致性的所述移动机器人的位姿估计包括如下步骤：
S51：确定待优化变量χ包含从时刻i到j的所述场景图像的连续关键帧图像和所述关键帧图像观测到的n个路标点：
Figure BDA0002882749760000152
其中，
Figure BDA0002882749760000156
表示时刻k对应的机体坐标系{Bk}下关键帧图像估计信息，
Figure BDA0002882749760000157
表示世界坐标系{W}下滑动窗口内第l个地图点的3D坐标；
由所述状态向量得到对应的误差状态向量：
Figure BDA0002882749760000153
S52：确定损失函数为如下形式：
Figure BDA0002882749760000154
其中，rp、
Figure BDA0002882749760000155
分别表示先验误差、惯性测量单元和轮速编码器测量值误差和重投影误差，∑p、∑Oij、∑Cij则为对应的协方差矩阵；
S53：求解所述损失函数。
在本发明的一种实施例中，采用Levenberg–Marquardt方法求解所述损失函数。
如图6所示，本发明还提供一种移动机器人的位姿估计系统，用于实现上述的位姿估计方法，包括系统前端和系统后端，系统前端主要是对采集数据处理，得到初步的位姿估计结果，系统后端对位姿估计结果再优化以减小误差。
本发明提供的移动机器人的位姿估计方法中，步骤1-4视为系统前端中重要环节，步骤5作为系统后端中重要环节。其中，图像采集设备采集数据、惯性测量单元IMU测量数据、轮式编码器测量数据三者共同作为前端输入，通过步骤1-4获得较为精确的位姿估计；进一步地，根据当前帧图像与关键帧序列的共视关系决定是否将当前帧作为关键帧；而关键帧序列为系统后端的输入，通过步骤5优化关键帧及观测路标点，以提高建图结果的一致性，实现高精度的位姿估计与地图估计。
在本发明一种具体的实施例中，基于本发明内容的方法和系统，本实施例通过C++编程在公开数据集上验证，对所得到的的定位结果进行综合分析，客观评价本发明的算法性能。
在本实施例中，所用的公开数据集为苏黎世联邦理工学院的Eucro数据集，该数据集主要收集了微型飞行器在室内场景的运动数据，该飞行器的传感器装置安装如图7所示，包括相机传感器、惯性测量单元、轮式编码器，每类传感器都有各自的传感器参考坐标系(Sensor Frame)，其中飞行器的机体坐标系(Body Frame)以IMU为中心，换言之，IMU的传感器参考坐标系就是飞行器的机体坐标系，而各传感器参考坐标系的转换通过旋转矩阵T来表示。
本实施例中采用了其中4个测试数据集，具体为：MH_01_easy、MH_02_easy、V1_02_medium和V2_03_difficult。本实验涉及的硬件如下：处理器为Intel Core i7-7700U CPU，3.60GHz×8、操作系统为Ubuntu 16.04LTS。实验代码调用的依赖库包括：计算机视觉库OpenCV、非线性优化库G2O、矩阵运算库Eigen。
如图8(a)-图8(d)所示，从上述四个数据集的估计轨迹绘图结果来看，虽然在不同的测试数据集上估计轨迹与真实估计轨迹相比，差异略有不同，但估计轨迹运动趋势与真实轨迹基本一致，由此可以验证本发明的有效性。
如图9(a)-图9(d)所示，从3D位置坐标的误差分析来看，XYZ三维位置误差围绕零均值波动，且波动趋势较为平滑，说明估计结果较为准确且收敛，再次验证了本发明的有效性。
如表1所示，从RMSE数值误差对比来看，本申请方法比现有技术方法具有更高的定位精度，定位误差降低约20％，直观说明了本申请发明内容的准确性。
表1定量实验结果
Figure BDA0002882749760000171
本申请实施例还提供一种控制装置，包括处理器和用于存储计算机程序的存储介质；其中，处理器用于执行所述计算机程序时至少执行如上所述的方法。
本申请实施例还提供一种存储介质，用于存储计算机程序，该计算机程序被执行时至少执行如上所述的方法。
本申请实施例还提供一种处理器，所述处理器执行计算机程序，至少执行如上所述的方法。
所述存储介质可以由任何类型的易失性或非易失性存储设备、或者它们的组合来实现。其中，非易失性存储器可以是只读存储器(ROM，Read Only Memory)、可编程只读存储器(PROM，Programmable Read-Only Memory)、可擦除可编程只读存储器(EPROM，ErasableProgrammable Read-Only Memory)、电可擦除可编程只读存储器(EEPROM，ElectricallyErasable Programmable Read-Only Memory)、磁性随机存取存储器(FRAM，FerromagneticRandom Access Memory)、快闪存储器(Flash Memory)、磁表面存储器、光盘、或只读光盘(CD-ROM，Compact Disc Read-Only Memory)；磁表面存储器可以是磁盘存储器或磁带存储器。易失性存储器可以是随机存取存储器(RAM，Random Access Memory)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器(SRAM，Static Random Access Memory)、同步静态随机存取存储器(SSRAM，SynchronousStatic Random Access Memory)、动态随机存取存储器(DRAM，Dynamic Random AccessMemory)、同步动态随机存取存储器(SDRAM，Synchronous Dynamic Random AccessMemory)、双倍数据速率同步动态随机存取存储器(DDRSDRAM，Double Data RateSynchronous Dynamic Random Access Memory)、增强型同步动态随机存取存储器(ESDRAMEnhanced Synchronous Dynamic Random Access Memory)、同步连接动态随机存取存储器(SLDRAM，Sync Link Dynamic Random Access Memory)、直接内存总线随机存取存储器(DRRAM，Direct Rambus Random Access Memory)。本发明实施例描述的存储介质旨在包括但不限于这些和任意其它适合类型的存储器。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统和方法，可以通过其它的方式实现。以上所描述的设备实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，如：多个单元或组件可以结合，或可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的各组成部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口，设备或单元的间接耦合或通信连接，可以是电性的、机械的或其它形式的。
上述作为分离部件说明的单元可以是、或也可以不是物理上分开的，作为单元显示的部件可以是、或也可以不是物理单元，即可以位于一个地方，也可以分布到多个网络单元上；可以根据实际的需要选择其中的部分或全部单元来实现本实施例方案的目的。
另外，在本发明各实施例中的各功能单元可以全部集成在一个处理单元中，也可以是各单元分别单独作为一个单元，也可以两个或两个以上单元集成在一个单元中；上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。
本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
或者，本发明上述集成的单元如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分。而前述的存储介质包括：移动存储设备、ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。
本申请所提供的几个方法实施例中所揭露的方法，在不冲突的情况下可以任意组合，得到新的方法实施例。
本申请所提供的几个产品实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的产品实施例。
本申请所提供的几个方法或设备实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的方法实施例或设备实施例。
以上内容是结合具体的优选实施方式对本发明所做的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims (10)

1.一种移动机器人的位姿估计方法，其特征在于，包括如下步骤：
S1：获取多源传感器数据，所述多源传感器数据包括：图像采集设备采集地场景图像、惯性测量单元采集地数据、轮式编码器采集地数据；
S2：根据所述场景图像检测场景动态将所述移动机器人的位姿估计初始化；
S3：所述惯性测量单元采集地数据、所述轮式编码器采集地数据通过状态转移方程计算所述移动机器人当前时刻状态向量的先验估计量及对应的协方差传递矩阵；
S4：根据所述场景图像对所述当前时刻状态向量的先验估计量及对应的协方差矩阵迭代至收敛完成滤波更新。
2.如权利要求1所述的移动机器人的位姿估计方法，其特征在于，还包括如下步骤：
S5：通过滑动窗口优化校准得到具有一致性的所述移动机器人的位姿估计。
3.如权利要求1所述的移动机器人的位姿估计方法，其特征在于，图像采集设备采集地场景图像包括当前时刻的图像帧；
惯性测量单元采集地数据包括角速度和加速度；
轮式编码器采集地数据包括所述移动机器人的左、右两轮的行进速度。
4.如权利要求2所述的移动机器人的位姿估计方法，其特征在于，根据所述场景图像检测场景动态将所述移动机器人的位姿估计初始化包括：
根据所述场景图像的连续图像帧进行角点检测估计匹配点对的运动状态进而确定所述移动机器人的状态；
若所述移动机器人处于运动状态，则通过所述图像采集设备、所述惯性测量单元、所述轮式编码器进行状态估计完成所述移动机器人的位姿估计初始化；
若所述移动机器人处于静止状态，则通过所述惯性测量单元完成所述移动机器人的位姿估计初始化。
5.如权利要求4所述的移动机器人的位姿估计方法，其特征在于，所述惯性测量单元包括陀螺仪和加速度计，通过所述惯性测量单元完成所述移动机器人的位姿估计初始化包括如下步骤：
将陀螺仪偏差bias bgk设定为所述陀螺仪的静态数据，加速度计偏差bias bak、bek设为0：
bgk＝mean(arrygyro)
bak＝0
bek＝0
其中，mean(arrygyro)表示陀螺仪静态测量数据的均值；
根据加速度计静态数据和重力向量计算从机体坐标系{Bk}到世界坐标系{W}的旋转四元数
Figure FDA0002882749750000021
gb＝mean(arrygyro)
gw＝(0 0 -9.8)T
Figure FDA0002882749750000022
其中，mean(arrygyro)表示加速度计静态测量数据的均值，gw表示世界坐标系{W}下的重力向量，运算符⊙表示向量做差后的四元数转化；
将所述状态向量中的其余量均设为0，完成初始化。
6.如权利要求5所述的移动机器人的位姿估计方法，其特征在于，所述惯性测量单元采集地数据、所述轮式编码器采集地数据通过状态转移方程计算所述移动机器人当前时刻状态向量的先验估计量及对应的协方差传递矩阵包括如下步骤：
S31：确定所述移动机器人当前时刻状态向量，所述当前时刻k的状态向量Xk由所述惯性测量单元及所述轮式编码器采集地所述移动机器人运动状态
Figure FDA0002882749750000023
和路标点的位置
Figure FDA0002882749750000024
两部分构成：
Figure FDA0002882749750000025
Figure FDA0002882749750000026
Figure FDA0002882749750000027
其中，
Figure FDA0002882749750000028
表示时刻k从机体坐标系{Bk}到世界坐标系{W}的旋转四元数，
Figure FDA0002882749750000031
和
Figure FDA0002882749750000032
表示以世界坐标系{W}为参考，机体坐标系{Bk}的3D位置与运动速度，
Figure FDA0002882749750000033
表示时刻k机体坐标系{Bk}到世界坐标系{W}的轮式编码器移动距离，bgk、bak、bek分别为陀螺仪、加速度计、轮式编码器偏差bias，
Figure FDA0002882749750000034
为世界坐标系{W}下路标点l的位置，ρ记为该路标点的逆深度；
S32：由所述状态向量根据运动学模型进行离散化处理得到离散时间下状态转移方程：
Figure FDA0002882749750000035
Figure FDA0002882749750000036
Figure FDA0002882749750000037
Figure FDA0002882749750000038
Figure FDA0002882749750000039
Figure FDA00028827497500000310
Figure FDA00028827497500000311
其中，
Figure FDA00028827497500000312
表示从时刻k-1到时刻k时，从机体坐标系{Bk}到世界坐标系{W}旋转四元数、3D位置、运动速度、移动距离的先验估计值，
Figure FDA00028827497500000313
分别为从时刻k-1到时刻k时惯性测量单元的陀螺仪、加速度计和轮式编码器的随机噪声先验估计值，
Figure FDA00028827497500000314
Figure FDA00028827497500000315
表示时刻k-1惯性测量单元中陀螺仪、加速度计的测量值，gW表示世界坐标系{W}下的重力向量，
Figure FDA00028827497500000316
为时刻k-1从机体坐标系{Bk}到世界坐标系{W}旋转矩阵估计值，Δt表示时间间隔；
S33：对所述状态向量进行Sigma点采样，并进行采样点的无迹变换：
Figure FDA0002882749750000041
其中，
Figure FDA0002882749750000042
为k-1时刻状态向量的第i个采样点，
Figure FDA0002882749750000043
为k-1时刻状态向量的均值，λ＝α2(n+κ)-n为决定Sigma点集缩放比例的参数，α代表Sigma点集到均值之间的距离，n为状态向量的维度，κ一般设为0或3-n，以保证n+κ＝3，而L为k-1时刻状态向量协方差矩阵Pk-1的Cholesky分解，即P＝L×LT；
所述Sigma点对应权值为：
Figure FDA0002882749750000044
Figure FDA0002882749750000045
Figure FDA0002882749750000046
Figure FDA0002882749750000047
其中，Wm为Sigma点均值的权值，WC为Sigma点协方差的权值；
S34：通过所述状态转移方程对所述状态向量进行时间更新，由k-1时刻的状态向量和协方差矩阵预测k时刻的状态向量先验估计和协方差矩阵：
Figure FDA0002882749750000048
Figure FDA0002882749750000049
Figure FDA00028827497500000410
其中，f(·)表示状态转移方程，为非线性函数；Qk表示k时刻下系统噪声方差矩阵。
7.如权利要求6所述的移动机器人的位姿估计方法，其特征在于，根据所述场景图像对所述当前时刻状态向量的先验估计量及对应的协方差矩阵迭代至收敛完成滤波更新包括如下步骤：
S41：将采用逆深度模型来表征所述当前时刻状态向量中路标点的位置
Figure FDA0002882749750000051
Figure FDA0002882749750000052
其中，
Figure FDA0002882749750000053
为世界坐标系{W}下路标点l的位置，ρ记为该路标点的逆深度，y(·)为表征函数；
S42：根据所述路标点的位置通过反投影观测方程预测观测量
Figure FDA0002882749750000054
Figure FDA0002882749750000055
其中，
Figure FDA0002882749750000056
表示时刻k从机体坐标系{Bk}到世界坐标系{W}的旋转矩阵，
Figure FDA0002882749750000057
表示从图像采集设备坐标系{C}到机体坐标系{B}的旋转矩阵，σkl表示k时刻路标点l的观测噪声，π(·)表示由图像采集设备内参决定的投影函数。
S43：根据所述观测量进行无迹变换并计算所述观测量的均值和协方差矩阵：
Figure FDA0002882749750000058
Figure FDA0002882749750000059
Figure FDA00028827497500000510
S44：根据所述先验估计和所述观测量的均值后验估计得所述计算卡尔曼增益K、更新状态向量Xk和协方差矩阵Pk：
Kk＝Tk|k-1Sk|k-1 -1
Figure FDA00028827497500000511
Pk＝Pk|k-1-KkSk|k-1Kk T
S45：迭代循环上述步骤S42-S44，直至状态向量Xk和协方差矩阵Pk收敛。
8.如权利要求7所述的移动机器人的位姿估计方法，其特征在于，通过滑动窗口优化校准得到具有一致性的所述移动机器人的位姿估计包括如下步骤：
S51：确定待优化变量χ包含从时刻i到j的所述场景图像的连续关键帧图像和所述关键帧图像观测到的n个路标点：
Figure FDA0002882749750000061
其中，
Figure FDA0002882749750000062
表示时刻k对应的机体坐标系{Bk}下关键帧图像估计信息，
Figure FDA0002882749750000063
表示世界坐标系{W}下滑动窗口内第l个地图点的3D坐标；
由所述状态向量得到对应的误差状态向量：
Figure FDA0002882749750000064
S52：确定损失函数为如下形式：
Figure FDA0002882749750000065
其中，rp、
Figure FDA0002882749750000066
分别表示先验误差、惯性测量单元和轮速编码器测量值误差和重投影误差，∑p、∑Oij、∑Cij则为对应的协方差矩阵；
S53：求解所述损失函数。
9.如权利要求8所述的移动机器人的位姿估计方法，其特征在于，采用Levenberg–Marquardt方法求解所述损失函数。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-9任一所述方法的步骤。
CN202110003763.1A 2021-01-04 2021-01-04 移动机器人的位姿估计方法及计算机可读存储介质 Active CN112815939B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110003763.1A CN112815939B (zh) 2021-01-04 2021-01-04 移动机器人的位姿估计方法及计算机可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110003763.1A CN112815939B (zh) 2021-01-04 2021-01-04 移动机器人的位姿估计方法及计算机可读存储介质

Publications (2)

Publication Number Publication Date
CN112815939A true CN112815939A (zh) 2021-05-18
CN112815939B CN112815939B (zh) 2024-02-23

Family

ID=75856974

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110003763.1A Active CN112815939B (zh) 2021-01-04 2021-01-04 移动机器人的位姿估计方法及计算机可读存储介质

Country Status (1)

Country Link
CN (1) CN112815939B (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113375658A (zh) * 2021-06-15 2021-09-10 电子科技大学中山学院 移动机器人故障下同时fdd和slam的方法及系统
CN113435386A (zh) * 2021-07-08 2021-09-24 浙江吉利控股集团有限公司 一种视觉车位无损滤波方法、装置及存储介质
CN113503872A (zh) * 2021-06-03 2021-10-15 浙江工业大学 一种基于相机与消费级imu融合的低速无人车定位方法
CN113724418A (zh) * 2021-08-26 2021-11-30 广州小鹏自动驾驶科技有限公司 一种数据处理方法、装置和可读存储介质
CN114509071A (zh) * 2022-04-20 2022-05-17 中国空气动力研究与发展中心低速空气动力研究所 一种风洞试验模型姿态测量方法
CN114625249A (zh) * 2022-03-07 2022-06-14 清华大学 一种检测捏合释放动作的方法、装置及电子设备
WO2023143132A1 (zh) * 2022-01-29 2023-08-03 北京三快在线科技有限公司 传感器数据的标定

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102042835A (zh) * 2010-11-05 2011-05-04 中国海洋大学 自主式水下机器人组合导航系统
CN109764880A (zh) * 2019-02-19 2019-05-17 中国科学院自动化研究所 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统
US20200011668A1 (en) * 2018-07-09 2020-01-09 Samsung Electronics Co., Ltd. Simultaneous location and mapping (slam) using dual event cameras
CN111136660A (zh) * 2020-02-19 2020-05-12 清华大学深圳国际研究生院 机器人位姿定位方法及系统
CN111750853A (zh) * 2020-06-24 2020-10-09 国汽(北京)智能网联汽车研究院有限公司 一种地图建立方法、装置及存储介质

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102042835A (zh) * 2010-11-05 2011-05-04 中国海洋大学 自主式水下机器人组合导航系统
US20200011668A1 (en) * 2018-07-09 2020-01-09 Samsung Electronics Co., Ltd. Simultaneous location and mapping (slam) using dual event cameras
CN109764880A (zh) * 2019-02-19 2019-05-17 中国科学院自动化研究所 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统
CN111136660A (zh) * 2020-02-19 2020-05-12 清华大学深圳国际研究生院 机器人位姿定位方法及系统
CN111750853A (zh) * 2020-06-24 2020-10-09 国汽(北京)智能网联汽车研究院有限公司 一种地图建立方法、装置及存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
文婉欣，等: "一种基于单目视觉的智能机械臂系统", 石河子科技, no. 6, pages 33 - 35 *

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113503872A (zh) * 2021-06-03 2021-10-15 浙江工业大学 一种基于相机与消费级imu融合的低速无人车定位方法
CN113503872B (zh) * 2021-06-03 2024-04-12 浙江工业大学 一种基于相机与消费级imu融合的低速无人车定位方法
CN113375658A (zh) * 2021-06-15 2021-09-10 电子科技大学中山学院 移动机器人故障下同时fdd和slam的方法及系统
CN113375658B (zh) * 2021-06-15 2023-05-09 电子科技大学中山学院 移动机器人故障下同时fdd和slam的方法及系统
CN113435386A (zh) * 2021-07-08 2021-09-24 浙江吉利控股集团有限公司 一种视觉车位无损滤波方法、装置及存储介质
CN113724418A (zh) * 2021-08-26 2021-11-30 广州小鹏自动驾驶科技有限公司 一种数据处理方法、装置和可读存储介质
CN113724418B (zh) * 2021-08-26 2023-07-04 广州小鹏自动驾驶科技有限公司 一种数据处理方法、装置和可读存储介质
WO2023143132A1 (zh) * 2022-01-29 2023-08-03 北京三快在线科技有限公司 传感器数据的标定
CN114625249A (zh) * 2022-03-07 2022-06-14 清华大学 一种检测捏合释放动作的方法、装置及电子设备
CN114625249B (zh) * 2022-03-07 2024-04-19 清华大学 一种检测捏合释放动作的方法、装置及电子设备
CN114509071A (zh) * 2022-04-20 2022-05-17 中国空气动力研究与发展中心低速空气动力研究所 一种风洞试验模型姿态测量方法

Also Published As

Publication number Publication date
CN112815939B (zh) 2024-02-23

Similar Documents

Publication Publication Date Title
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
CN106679648B (zh) 一种基于遗传算法的视觉惯性组合的slam方法
Neuhaus et al. Mc2slam: Real-time inertial lidar odometry using two-scan motion compensation
Lupton et al. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions
US9071829B2 (en) Method and system for fusing data arising from image sensors and from motion or position sensors
Laidlow et al. Dense RGB-D-inertial SLAM with map deformations
CN108036785A (zh) 一种基于直接法与惯导融合的飞行器位姿估计方法
CN107909614B (zh) 一种gps失效环境下巡检机器人定位方法
US20220051031A1 (en) Moving object tracking method and apparatus
CN109238277B (zh) 视觉惯性数据深度融合的定位方法及装置
Zhang et al. Vision-aided localization for ground robots
CN115371665B (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN111145251A (zh) 一种机器人及其同步定位与建图方法和计算机存储设备
CN114529576A (zh) 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
Aufderheide et al. Towards real-time camera egomotion estimation and three-dimensional scene acquisition from monocular image streams
Caruso et al. Robust indoor/outdoor navigation through magneto-visual-inertial optimization-based estimation
CN113052855B (zh) 一种基于视觉-imu-轮速计融合的语义slam方法
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN109785428A (zh) 一种基于多态约束卡尔曼滤波的手持式三维重建方法
CN112284381B (zh) 视觉惯性实时初始化对准方法及系统
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
CN115235454B (zh) 行人运动约束的视觉惯性融合定位与建图方法和装置
Irmisch et al. Simulation framework for a visual-inertial navigation system
De Marco et al. Position, velocity, attitude and accelerometer-bias estimation from imu and bearing measurements
Cen et al. A low-cost visual inertial odometry for mobile vehicle based on double stage Kalman filter

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