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

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

【技术实现步骤摘要】
移动机器人的位姿估计方法及计算机可读存储介质


[0001]本专利技术涉及机器人定位
，尤其涉及一种移动机器人的位姿估计方法及计算机可读存储介质。

技术介绍

[0002]同时定位与地图构建(simultaneous localization and mapping，SLAM)是赋予机器人智能感知的关键技术之一，该技术能获得移动机器人本身的位置和周围环境的地图。经过几十年的研究发展，SLAM问题融合了概率估计、射影几何、信号处理、数值优化等多个领域的理论与研究成果，但究其本质，仍可视为由运动方程和观测方程构建的状态估计问题。SLAM的运动方程含义为从k
‑
1时刻到k时刻，机器人的位置变化情况。数学上可表示为：
[0003]x
k
＝f(x
k
‑1，μ
k
，ω
k
)
[0004]其中，μ
k
为传感器测量读数，ω
k
为噪声。
[0005]SLAM的观测方程含义为k时刻，机器人于位置x
k
处探测到了某一个路标y
j
，产生一个观测数据z
k，j
：
[0006]z
k，j
＝h(y
j
，x
k
，v
k，j
)
[0007]其中，v
k，j
为观测噪声。
[0008]故SLAM问题的研究的是，在知道运动测量的读数μ以及传感器读数z时，如何求解定位问题(估计x)与建图问题(估计y)。
[0009]目前SLAM主要有两种实现途径，基于激光传感器和基于视觉传感器。激光SLAM虽然可以实现更高的定位精度，但激光传感器的要求较高，也探测范围受到限制；而视觉SLAM对传感器要求较低，探测范围不受限制，因此更加适用于移动机器人的定位场景。但纯视觉对于运动模糊、遮挡、光照变化等条件十分敏感，严重影响定位精度，因此通过多源传感器融合的方式降低定位误差。
[0010]较为棘手的是，SLAM问题是一个非线性问题，在位姿估计的过程中，计算过程较为复杂。一般情况下，通过线性化的方式近似求解非线性问题，因此在求解精度上有一定牺牲；虽然采用无迹卡尔曼滤波通过拟合状态变量的概率密度分布代替将非线性问题线性化，可以提高求解精度，但使用的数据来源可能只有图像采集设备和IMU(Inertial Measurement Unit，惯性测量单元)或者雷达和图像采集设备的数据，精度依然不高。
[0011]现有技术中缺乏一种高精度的位姿估计方法。
[0012]以上
技术介绍
内容的公开仅用于辅助理解本专利技术的构思及技术方案，其并不必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请的申请日已经公开的情况下，上述
技术介绍
不应当用于评价本申请的新颖性和创造性。

技术实现思路

[0013]本专利技术为了解决现有的问题，提供一种移动机器人的位姿估计方法及计算机可读
存储介质。
[0014]为了解决上述问题，本专利技术采用的技术方案如下所述：
[0015]一种移动机器人的位姿估计方法，包括如下步骤：S1：获取多源传感器数据，所述多源传感器数据包括：图像采集设备采集地场景图像、惯性测量单元采集地数据、轮式编码器采集地数据；S2：根据所述场景图像检测场景动态将所述移动机器人的位姿估计初始化；S3：所述惯性测量单元采集地数据、所述轮式编码器采集地数据通过状态转移方程计算所述移动机器人当前时刻状态向量的先验估计量及对应的协方差传递矩阵；S4：根据所述场景图像对所述当前时刻状态向量的先验估计量及对应的协方差矩阵迭代至收敛完成滤波更新。
[0016]优选地，还包括如下步骤：S5：通过滑动窗口优化校准得到具有一致性的所述移动机器人的位姿估计。
[0017]优选地，图像采集设备采集地场景图像包括当前时刻的图像帧；惯性测量单元采集地数据包括角速度和加速度；轮式编码器采集地数据包括所述移动机器人的左、右两轮的行进速度。
[0018]优选地，根据所述场景图像检测场景动态将所述移动机器人的位姿估计初始化包括：根据所述场景图像的连续图像帧进行角点检测估计匹配点对的运动状态进而确定所述移动机器人的状态；若所述移动机器人处于运动状态，则通过所述图像采集设备、所述惯性测量单元、所述轮式编码器进行状态估计完成所述移动机器人的位姿估计初始化；若所述移动机器人处于静止状态，则通过所述惯性测量单元完成所述移动机器人的位姿估计初始化。
[0019]优选地，所述惯性测量单元包括陀螺仪和加速度计，通过所述惯性测量单元完成所述移动机器人的位姿估计初始化包括如下步骤：将陀螺仪偏差bias b
gk
设定为所述陀螺仪的静态数据，加速度计偏差bias b
ak
、b
ek
设为0：
[0020]b
gk
＝mean(arry
gyro
)
[0021]b
ak
＝0
[0022]b
ek
＝0
[0023]其中，mean(arry
gyro
)表示陀螺仪静态测量数据的均值；
[0024]根据加速度计静态数据和重力向量计算从机体坐标系{B
k
}到世界坐标系{W}的旋转四元数
[0025]g
b
＝mean(arry
gyro
)
[0026]g
w
＝(0 0
ꢀ‑
9.8)
T
[0027][0028]其中，mean(arry
gyro
)表示加速度计静态测量数据的均值，g
w
表示世界坐标系{W}下的重力向量，运算符
⊙
表示向量做差后的四元数转化；
[0029]将所述状态向量中的其余量均设为0，完成初始化。
[0030]优选地，所述惯性测量单元采集地数据、所述轮式编码器采集地数据通过状态转移方程计算所述移动机器人当前时刻状态向量的先验估计量及对应的协方差传递矩阵包括如下步骤：S31：确定所述移动机器人当前时刻状态向量，所述当前时刻k的状态向量X
k
由
所述惯性测量单元及所述轮式编码器采集地所述移动机器人运动状态和路标点的位置两部分构成：
[0031][0032][0033][0034]其中，表示时刻k从机体坐标系{B
k
}到世界坐标系{W}的旋转四元数，和表示以世界坐标系{W}为参考，机体坐标系{B
k
}的3D位置与运动速度，表示时刻k机体坐标系{B
k
}到世界坐标系{W}的轮式编码器移动距离，b
gk
、b
ak
、b
ek
分别为陀螺仪、加速度计、轮式编码器偏差bias，为世界坐本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种移动机器人的位姿估计方法，其特征在于，包括如下步骤：S1：获取多源传感器数据，所述多源传感器数据包括：图像采集设备采集地场景图像、惯性测量单元采集地数据、轮式编码器采集地数据；S2：根据所述场景图像检测场景动态将所述移动机器人的位姿估计初始化；S3：所述惯性测量单元采集地数据、所述轮式编码器采集地数据通过状态转移方程计算所述移动机器人当前时刻状态向量的先验估计量及对应的协方差传递矩阵；S4：根据所述场景图像对所述当前时刻状态向量的先验估计量及对应的协方差矩阵迭代至收敛完成滤波更新。2.如权利要求1所述的移动机器人的位姿估计方法，其特征在于，还包括如下步骤：S5：通过滑动窗口优化校准得到具有一致性的所述移动机器人的位姿估计。3.如权利要求1所述的移动机器人的位姿估计方法，其特征在于，图像采集设备采集地场景图像包括当前时刻的图像帧；惯性测量单元采集地数据包括角速度和加速度；轮式编码器采集地数据包括所述移动机器人的左、右两轮的行进速度。4.如权利要求2所述的移动机器人的位姿估计方法，其特征在于，根据所述场景图像检测场景动态将所述移动机器人的位姿估计初始化包括：根据所述场景图像的连续图像帧进行角点检测估计匹配点对的运动状态进而确定所述移动机器人的状态；若所述移动机器人处于运动状态，则通过所述图像采集设备、所述惯性测量单元、所述轮式编码器进行状态估计完成所述移动机器人的位姿估计初始化；若所述移动机器人处于静止状态，则通过所述惯性测量单元完成所述移动机器人的位姿估计初始化。5.如权利要求4所述的移动机器人的位姿估计方法，其特征在于，所述惯性测量单元包括陀螺仪和加速度计，通过所述惯性测量单元完成所述移动机器人的位姿估计初始化包括如下步骤：将陀螺仪偏差bias b
gk
设定为所述陀螺仪的静态数据，加速度计偏差bias b
ak
、b
ek
设为0：b
gk
＝mean(arry
gyro
)b
ak
＝0b
ek
＝0其中，mean(arry
gyro
)表示陀螺仪静态测量数据的均值；根据加速度计静态数据和重力向量计算从机体坐标系{B
k
}到世界坐标系{W}的旋转四元数g
b
＝mean(arry
gyro
)g
w
＝(0 0
ꢀ‑
9.8)
T
其中，mean(arry
gyro
)表示加速度计静态测量数据的均值，g
w
表示世界坐标系{W}下的重力向量，运算符
⊙
表示向量做差后的四元数转化；
将所述状态向量中的其余量均设为0，完成初始化。6.如权利要求5所述的移动机器人的位姿估计方法，其特征在于，所述惯性测量单元采集地数据、所述轮式编码器采集地数据通过状态转移方程计算所述移动机器人当前时刻状态向量的先验估计量及对应的协方差传递矩阵包括如下步骤：S31：确定所述移动机器人当前时刻状态向量，所述当前时刻k的状态向量X
k
由所述惯性测量单元及所述轮式编码器采集地所述移动机器人运动状态和路标点的位置两部分构成：分构成：分构成：其中，表示时刻k从机体坐标系{B
k
}到世界坐标系{W}的旋转四元数，和表示以世界坐标系{W}为参考，机体坐标系{B
k
}的3D位置与运动速度，表示时刻k机体坐标系{B
k
}到世界坐标系{W}的轮式编码器移动距离，b
gk
、b
ak
、b
ek
分别为陀螺仪、加速度计、轮式编码器偏差bias，为世界坐标系{W}下路标点l的位置，ρ记为该路标点的逆深度；S32：由所述状态向量根据运动学模型进行离散化处理得到离散时间下状态转移方程：S32：由所述状态向量根据运动学模型进行离散化处理得到离散时间下状态转移方程：S32：由所述状态向量根据运动学模型进行离散化处理得到离散时间下状态转移方程：S32：由所述状态向量根据运动学模型进行离散化处理得到离散时间下状态转移方程：S32：由所述状态向量根据运动学模型进行离散化处理得到离散时间下状态转移方程：S32：由所述状态向量根据运动学模型进行离散化处理得到离散时间下状态转移方程：S32：由所述状态向量根据运动学模型进行离散化处理得到离散时间下状态转移方程：其中，表示从时刻k
‑
1到时刻k时，从机体坐标系{B
k
}到世界坐标系{W}旋转四元数、3D位置、运动速度、移动距离的先验估计值，分别为从时刻k
‑
1到时刻k时惯性测量单元的陀螺仪、加速度计和轮式编码器...

【专利技术属性】
技术研发人员：张凯，黄鑫，董宇涵，
申请(专利权)人：清华大学深圳国际研究生院，
类型：发明
国别省市：