CN113804184A - 基于多传感器的地面机器人定位方法 - Google Patents
基于多传感器的地面机器人定位方法 Download PDFInfo
- Publication number
- CN113804184A CN113804184A CN202010544006.0A CN202010544006A CN113804184A CN 113804184 A CN113804184 A CN 113804184A CN 202010544006 A CN202010544006 A CN 202010544006A CN 113804184 A CN113804184 A CN 113804184A
- Authority
- CN
- China
- Prior art keywords
- robot
- dimensional code
- encoder
- image
- positioning
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 31
- 230000033001 locomotion Effects 0.000 claims abstract description 23
- 230000004927 fusion Effects 0.000 claims abstract description 19
- 238000001514 detection method Methods 0.000 claims abstract description 17
- 238000005259 measurement Methods 0.000 claims abstract description 15
- 238000012937 correction Methods 0.000 claims abstract description 12
- 238000005457 optimization Methods 0.000 claims abstract description 9
- 238000005516 engineering process Methods 0.000 claims description 17
- 230000000007 visual effect Effects 0.000 claims description 14
- 238000012545 processing Methods 0.000 claims description 13
- 230000010354 integration Effects 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 9
- 238000003384 imaging method Methods 0.000 claims description 8
- 101100218497 Haliangium ochraceum (strain DSM 14365 / JCM 11303 / SMP-2) barP gene Proteins 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 4
- 238000007781 pre-processing Methods 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000006835 compression Effects 0.000 claims description 3
- 238000007906 compression Methods 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- NHXLMOGPVYXJNR-UHFFFAOYSA-N srif Chemical compound N1C(=O)C(C(C)O)NC(=O)C(CCCCN)NC(=O)C(CC=2C3=CC=CC=C3NC=2)NC(=O)C(CC=2C=CC=CC=2)NC(=O)C(CC=2C=CC=CC=2)NC(=O)C(CC(N)=O)NC(=O)C(CCCCN)NC(=O)C(NC(=O)CNC(=O)C(C)N)CSSCC(C(O)=O)NC(=O)C(CO)NC(=O)C(C(O)C)NC(=O)C1CC1=CC=CC=C1 NHXLMOGPVYXJNR-UHFFFAOYSA-N 0.000 claims description 3
- 238000007500 overflow downdraw method Methods 0.000 claims 1
- 238000004364 calculation method Methods 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000007688 edging Methods 0.000 description 1
- 238000011065 in-situ storage Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000003860 storage Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 239000002699 waste material Substances 0.000 description 1
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
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
Abstract
本发明涉及一种基于多传感器的地面机器人定位方法，包括以下步骤：在机器人上安装编码器、相机和IMU惯性测量单元；地面上铺设多个二维码作为地图标记；在机器人行进过程中，编码器里程计模块通过卡尔曼滤波器融合编码器和IMU数据提供连续惯性航迹推算,“视觉‑惯导”定位模块通过非线性优化优化算法融合图像、IMU、编码器里程计航迹和运动流形约束，求解出机器人六维位姿；二维码检测模块提供全局位姿校正。本发明高效的融合了环境纹理、二维码、惯性测量和编码器多种数据信息，不同数据类型采用不同的融合策略，提供高精度机器人六维位姿定位。
Description
技术领域
本发明涉及智能机器人领域，特别涉及一种基于多传感器的地面机器人定位方法。
背景技术
机器人定位是指机器人在移动过程中，通过采集的传感器数据，与地图进行信息匹配，从而获取自身在环境中的位姿。定位是机器人基本的模块，准确的自我定位是机器人导航的前提。
二维码定位是常见的一种定位方式，在仓储机器人、分拣机器人等方向应用广泛。预先在地面上铺设二维码，机器人在二维码组成的网格上运动；通过相机拍摄地面，识别图像中的二维码来实现机器人全局定位。两个二维码之间的路程通过IMU惯性测量单元和编码器来定位，从而给导航算法提供连续的轨迹信息。但是由于地面起伏、轮胎侧滑等原因的影响，IMU和编码器定位误差会迅速积累。从而导致机器人的轨迹误差和停止误差增大。
场景中的二维码是离散铺设的，在机器人行进过程中，能够检测到二维码的图像比例非常低，往往远低于1％。在没有铺设二维码的区域，相机拍摄的图像数据没能提供定位信息，这就造成了视觉信息的浪费。
如何在不增加成本的前提下，提高机器人上现有的传感器数据的利用率，改善定位精度和鲁棒性，是亟需解决的问题。
“视觉-惯导”里程计(visual-inertial odometry)技术通过融合单目相机和IMU惯性测量单元数据，能够提供真实尺度的定位信息。在无二维码的区域，可以使用“视觉-惯导”里程计技术来提供定位信息，提高视觉信息的利用率，并与编码器里程计的定位结果融合互补。
但是，“视觉-惯导”里程计因IMU的积分受到其初始值的影响，在地面机器人上的定位效果不好。地面机器人的运动往往比较平稳，多以匀加速直线运动、原地旋转运动为主，这种运动会导致“视觉-惯导”里程计的尺度信息无法观测。地面机器人的pitch和roll方向的变化非常小，也很难实现精确观测。机器人yaw方向角度也会随着时间漂移。
如何抑制“视觉-惯导”里程计在地面机器人应用中遇到的上述问题，是影响“惯导-视觉”里程计定位精度的关键问题。
在多传感器定位时，如何评估不同传感器的定位误差，根据何种策略来融合数据，都是影响融合效果的重要因素。标准的卡尔曼滤波器的融合策略并不适用所有情形，需要在实践中探索新的融合方案。
发明内容
鉴于上述技术中的不足，本发明提供了新的多传感器定位方法，该在不改变硬件配置的情况下，更有效的利用传感器数据，提高地面机器人定位精度及鲁棒性。
第一方面，通过“视觉-惯导”里程计技术使得每帧图像都能提供定位信息。当图像中存在二维码时，通过二维码检测算法提供全局位置信息校；当图像中不存在二维码时，则通过“视觉-惯导”里程计算法来提供连续定位。
第二方面，改进了“视觉-惯导”里程计技术，把编码器里程计的航迹数据和机器人运动流形约束加入到非线性优化中。编码器里程计轨迹能够提供真实尺度信息，而运动流形约束则对机器人的pitch和roll角度提供约束。从而消除了地面机器人的“视觉-惯导”里程计中尺度、pitch和roll这三个维度难以测量的问题。
第三方面，改进了多传感器定位融合策略。编码器和IMU数据融合策略采用融合增量值的方式，而不是融合绝对数值；增量值及其噪声方差仅对最近时间段的运动状态进行建模，建模精度更高。二维码定位噪声方差加入了位置相关项，二维码中心距离图像中心越远，定位噪声方差越大；该方差项体现了相机的成像特性，即离图像中心越远，相机的投影畸变和成像噪声越大。
基于多传感器的定位方法，包含以下步骤：
步骤s1，在机器人上安装相机和IMU惯性测量单元，在驱动轮上安装编码器，并标定传感器的内参和外参；
步骤s2，在地面上铺设二维码，并将二维码的全局坐标和尺寸信息进行编码；
步骤s3，机器人的初始位置放置在某个二维码上方，通过二维码定位来初始化机器人的位置；
步骤s4，在机器人行进过程中，采集图像数据、IMU数据和编码器数据，不同传感器具有不同的采样周期；
步骤s5，编码器里程计模块通过卡尔曼滤波器，增量融合IMU和编码器数据，进行航迹推演，推算出机器人连续三维位姿轨迹gPk(x,y,θ)；
步骤s6，“视觉-惯导”里程计定位模块结合图像、IMU、编码器里程计轨迹及运动流形约束；通过非线性优化技术求解出机器人的当前六维位姿vPosk(x,y,z,α,β,θ)；
步骤s7，二维码检测模块扫描图像，如果能检测出二维码，则根据二维码特征点求解出机器人的全局位姿bPosk(x,y,z,α,β,θ)，否则bPosk(x,y,z,α,β,θ)设为无效值；
步骤s8，利用卡尔曼滤波器融合“视觉-惯导”定位vPosk(x,y,z,α,β,θ)及二维码定位bPosk(x,y,z,α,β,θ)的值。融合后的位姿作为机器人的当前位姿。
多传感器定位方法，在硬件方面的操作有：标定机器人驱动轮直径，建立编码器读数增量到车轮移动距离的变换函数Tr(x)；标定机器人左右驱动轮的轮间距B；标定相机内参和外参；标定IMU的外参；对多个传感器需进行时间戳对齐。
编码器里程计的增量融合方案为：
假设时刻tk-1的机器人编码器里程计求解的三维位姿为gPk-1＝(x(k-1),y(k-1),θ(k-1))；
步骤s51，从IMU的陀螺仪来估计机器人方向角变化量,
陀螺仪提供角速度信息，通过对yaw方向角速度的积分，可以得到机器人相对上次定位时刻航向角变化量为：
其中ng(k)为陀螺仪角度测量的高斯噪声，该噪声的均值为零，方差为
随着时间线性增长；
步骤s52，从编码器来估计机器人航向角变化量，
根据编码器读数增量及变换函数Tr(x)，计算左轮行进距离ΔSl和右轮行驶距离ΔSr，则机器人走过的路程为ΔS＝(ΔSl+ΔSr)*0.5；
机器人的航向角变化量为：e(dθ)＝(ΔSr-ΔSl)/B+ne(k)
其中ne(k)为编码器测量的高斯噪声，该噪声的均值为零，方差为
随着机器人行走路程的增加线性增长；B为机器人左右轮间距；
步骤s53，融合航向角变化量，
增量融合g(dθ)和e(dθ)，融合结果作为编码器里程计的航向角变化量；根据卡尔曼滤波器原理，有：
方差σg(k)和σe(k)的值都是从上个采样时刻起开始估算。
而Δθ的方差为
在机器人静止和运动时，系数c1和c2是变化的；
步骤s54，推算机器人在二维平面上的位置变化量，
在每个很短的采样周期内，机器人的行走的路程非常小，因此可以将机器人的轨迹看作多个微小的直线段组成。则机器人在x,y坐标轴上的位置变化可以表示为：
Δx＝ΔS*cos(θ(k-1)+Δθ/2)
Δy＝ΔS*sin(θ(k-1)+Δθ/2)
步骤s55，航迹推演得到机器人在平面上的三维位姿，
x(k)＝x(k-1)+Δx
y(k)＝y(k-1)+Δy
θ(k)＝θ(k-1)+Δθ
在步骤s6中，“视觉-惯导”里程计定位的步骤为：
步骤s61，读取相机图像、IMU和编码器里程计轨迹数据，视觉跟踪模块检测并跟踪图像特征Features；预积分模块对IMU数据进行预积分处理；
步骤s62，参数初始化，获取尺度、重力、速度以及IMU bias信息；
步骤s63，使用滑动窗来维护最近的数据，滑动窗的数据包含有图像帧及其对应的图像特征features、IMU惯性测量数据及编码器里程计轨迹数据；滑动窗的长度是固定的，通过边缘化(marginalization)操作删除较旧或者不符合要求的数据，加入最新的数据；
步骤s64：通过非线性优化技术计算机器人的最优位姿vPosk(x,y,z,α,β,θ)，使其最符合滑动窗内的图像、IMU、编码器里程计数据以及机器人运动流形约束。
机器人通过二维码定位来初始化位置，如果机器人开机后检测不到二维码，则机器人会向服务器发送“位置初始化失败”信息，并通过机器人上的声光报警装置来报警显示。
在步骤s7中，二维码检测算法的具体步骤为：
步骤s71，对整幅图像做滤波及增强对比度处理；
步骤s72，利用相机内参，对图像做畸变校正处理；
步骤s73，对图像做二值化处理，生成二值图像；
步骤s74，扫描二值图像上，检测二维码；如果没有检测到二维码，则退出检测模块；如果检测到二维码，则继续下面的步骤；
步骤s75，将二维码图像区域变换为标准二维码图案，并根据二维码编码规则解码二维码图案，获取二维码的坐标信息barP(x,y,z,α,β,θ)；
步骤s76，检测二维码特征点，通过计算机视觉的pnp技术求解出相机相对二维码的位姿camPos(x,y,z,α,β,θ)；
步骤s77，根据相机的外参及相机相对二维码的位姿camPos(x,y,z,α,β,θ)推算出机器人相对二维码的位姿btPos(x,y,z,α,β,θ)；
步骤s78，通过二维码坐标位置barP(x,y,z,α,β,θ)及机器人相对二维码位姿btPos(x,y,z,α,β,θ)，推算出机器人的在地图中的全局位姿bPos(x,y,z,α,β,θ)＝barP(x,y,z,α,β,θ)+btPos(x,y,z,α,β,θ)。
若当前帧检测到二维码，则通过卡尔曼滤波器融合二维码定位和“视觉-惯导”里程计定位，实现全局位姿校正，包括，
首先，求解二维码定位的噪声方差：
其次，求解“视觉-惯导”定位的噪声方差：
ds为相邻两次检测到二维码的时间间隔内机器人走过的路程；
如果当前周期内没有检测到二维码，则机器人位置就是“视觉-惯导”里程计的输出值，即机器人当前位置Pos(k)＝vPos(k)；
如果当前周期检测到二维码，则根据卡尔曼滤波器融合机器人当前位置，即
本发明的有益效果如下：
1).通过“视觉-惯导”里程计技术，能够提供逐帧的视觉和惯导的定位融合，实现了对视觉信息的更有效应用。在没有二维码的区域利用“视觉-惯导”定位，在检测到二维码的时候，二维码提供机器人全局位姿信息，校正“视觉-惯导”定位的累积误差。
2).将编码器里程计和运动流形约束添加到“视觉-惯导”里程计技术中，提供了尺度信息及pitch、roll方向的约束，使得“视觉-惯导”里程计技术更适用地面机器人。
3).采用增量融合方式来融合编码器和IMU数据，更符合积分定位方式的模型，实现了更精确的航迹推演定位
4).本发明的多传感器机器人定位方法，融合了环境纹理、二维码、编码器和惯性测量单元的信息，实现了传感器数据的更高效利用，提高了定位的精度和鲁棒性。
附图说明
图1是整个多传感器定位算法的流程图。
图2是机器人由k-1时刻到k时刻的运动轨迹示意图。
图3为机器人位姿变换示意图。
图4为“视觉-惯导”里程计定位流程图。
图5示出了以QR码为例的二维码中心到图像中心的距离。
具体实施方式
下面参照附图来详细描述本发明的具体实施，但本实施例并不用于限制本发明，凡是采用本发明的相似结构及相似变化，均应列入本发明的保护范围。
多传感器定位方法，在硬件方面的操作有：标定机器人驱动轮直径，建立编码器读数增量到车轮移动距离的变换函数Tr(x)；标定机器人左右驱动轮的轮间距B；标定相机内参和外参；标定IMU的外参；对多个传感器需进行时间戳对齐。
图1示出了多传感器定位系统的组成及处理流程。主要步骤有：
步骤s1，在机器人上安装相机和IMU惯性测量单元，在驱动轮上安装编码器，并标定传感器的内参和外参；
步骤s2，在地面上铺设二维码，并将二维码的全局坐标和尺寸信息进行编码；
步骤s3，机器人的初始位置放置在某个二维码上方，通过二维码定位来初始化机器人的位置；
步骤s4，在机器人行进过程中，采集图像数据、IMU数据和编码器数据，不同传感器具有不同的采样周期；
步骤s5，编码器里程计模块通过卡尔曼滤波器，增量融合IMU和编码器数据，进行航迹推演，推算出机器人连续三维位姿轨迹gPk(x,y,θ)；
步骤s6，“视觉-惯导”里程计定位模块结合图像、IMU、编码器里程计轨迹及运动流形约束；通过非线性优化技术求解出机器人的当前六维位姿vPosk(x,y,z,α,β,θ)；
步骤s7，二维码检测模块扫描图像，如果能检测出二维码，则根据二维码特征点求解出机器人的全局位姿bPosk(x,y,z,α,β,θ)，否则bPosk(x,y,z,α,β,θ)设为无效值；
步骤s8，利用卡尔曼滤波器融合“视觉-惯导”定位vPosk(x,y,z,α,β,θ)及二维码定位bPosk(x,y,z,α,β,θ)的值。融合后的位姿作为机器人的当前位姿。
机器人通过二维码定位来初始化位置，如果机器人开机后检测不到二维码，则机器人会向服务器发送“位置初始化失败”信息，并通过机器人上的声光报警装置来报警显示。
图2示出了机器人从时刻k-1到时刻k的运动轨迹。在很短的时间间隔内，机器人的左右轮的运动速度认为匀速不变，则机器人的线速度和角速度保持不变。机器人看作沿着一个圆弧运动，该圆弧对应的圆心角等于机器人方向角的变化量Δθ。由于Δθ的值很小，因此机器人走过的路程圆弧ΔS近似等于该圆弧对应的弦Δd，即有ΔS＝Δd。
图3示出了机器人从时刻k-1到时刻k的位姿变化。步骤s5融合编码器和IMU传感器增量数据通过航迹推演方法来推算该位姿变化信息，主要步骤如下：
假设时刻tk-1的机器人编码器里程计求解的三维位姿为gPk-1＝(x(k-1),y(k-1),θ(k-1))；
步骤s51，从IMU的陀螺仪来估计机器人航向角变化量，
陀螺仪提供角速度信息，通过对yaw方向角速度的积分，可以得到机器人相对上次定位时刻方向角变化量为：
其中ng(k)为陀螺仪角度测量的高斯噪声，该噪声的均值为零，方差为
随着时间线性增长。
步骤s52，从编码器来估计机器人航向角变化，
根据编码器读数增量及变换函数Tr(x)，求解出左轮行进距离ΔSl及右轮行驶距离ΔSr，则机器人走过的路程为ΔS＝(ΔSl+ΔSr)*0.5；
机器人的方向角变化量为：
e(dθ)＝(ΔSr-ΔSl)/B+ne(k)，
其中ne(k)为编码器测量的高斯噪声，该噪声的方差为
随着机器人行走路程的增加线性增长。B为机器人左右轮间距。
步骤s53，融合航向角变化量，
融合g(dθ)和e(dθ)，融合结果作为编码器里程计的方向角变化量。根据卡尔曼滤波器原理，有：
在机器人运动过程中，σg(k)和σe(k)都是一直增加的。一直增加的数值在应用中是难以处理的，因此σg(k)和σe(k)的值都是从上个采样时刻起开始估算。
而Δθ的方差为
在机器人静止和运动时，系数c1和c2是变化的。
步骤s54，推算机器人在二维平面上的位置变化，
在每个很短的采样周期内，机器人的行走的路程非常小，因此可以将机器人的轨迹看作多个微小的直线段组成。则机器人在x,y坐标轴上的位置变化可以表示为：
Δx＝ΔS*cos(θ(k-1)+Δθ/2)
Δy＝ΔS*sin(θ(k-1)+Δθ/2)
步骤s55，航迹推演得到机器人三维位姿：
x(k)＝x(k-1)+Δx
y(k)＝y(k-1)+Δy
θ(k)＝θ(k-1)+Δθ
图4示出了步骤s6中“视觉-惯导”定位的具体算法流程：
步骤s61，预处理，
获取相机、IMU和编码器里程计轨迹数据。视觉跟踪模块检测并跟踪图像特征Features；预积分模块对IMU数据进行预积分处理；编码器里程计模块融合IMU和编码器数据进行航迹推演；得到当前时刻的位置、速度和旋转姿态。
步骤s62，初始化，
初始化需要获取尺度、重力、速度以及IMU bias信息。
由于视觉structure-from-motion(sfm)在初始化的过程中表现更好，因此初始化首先进行视觉sfm，以此来估计相机位姿及特征点位置。为了限制计算量，可以仅使用最近的一定数量帧，即使用滑动窗(slide window)。视觉sfm的步骤为：(1)将第一帧作为关键帧；(2)检测最新帧的特征并与关键帧进行特征匹配；如果有足够多的匹配特征数量及足够的视差，则使用5点法来恢复两帧之间的旋转及有尺度自由度的平移；(3)三角化特征点；(4)通过PnP技术来解算和跟踪图像特征features，计算两帧之间的相对位姿并三角化feature points；(5)对滑动窗中所有帧及3D feature points进行最小化重投影误差的全局束调整优化，优化每帧的位姿及3D feature points。
视觉sfm后，将IMU的预积分结果与视觉sfm对齐，求解初始化参数。初始化的参数有：陀螺仪的偏移量、初始化速度、重力向量和尺度因子。
步骤s63，数据滑动窗，
为了限制计算量，一般使用滑动窗来维护最近的数据。滑动窗的数据包含有图像帧及其对应的图像特征features和IMU惯性测量数据及编码器里程计数据。滑动窗的长度是固定的，通过边缘化(marginalization)操作删除较旧或者不符合要求的数据，加入最新的数据。
步骤s64，非线性优化，
建立“视觉-惯导”的集束调整方程C(x)，求解相机最优位姿解，使其最符合滑动窗内的图像、IMU、编码器里程计数据以及机器人运动流形约束。
运动流形约束可以表示为几何约束g(x)＝0。运动流形约束与地面地形有关；可选的，室内地面就可以认为是一个平面，那么机器人运动流形约束就是近似平面约束；在函数C(x)中加入
项即实现了机器人运动的流形约束。
获取相机位姿后，通过相机的外参就可以推导出机器人当前的位姿。
在机器人行进过程中，二维码检测算法会对扫描图像进行二维码检测。二维码检测算法具有以下步骤：
步骤s71，对整幅图像做滤波及增强对比度处理；
步骤s72，利用相机内参，对图像做畸变校正处理；
步骤s73，对图像做二值化处理，生成二值图像；
步骤s74，扫描二值图像上，检测二维码；如果没有检测到二维码，则退出检测模块；如果检测到二维码，则继续下面的步骤；
步骤s75，将二维码图像区域变换为标准二维码图案，并根据二维码编码规则解码二维码图案，获取二维码的全局坐标信息barP(x,y,z,α,β,θ)；
步骤s76，检测二维码特征点，通过计算机视觉的pnp技术求解出相机相对二维码的位姿camPos(x,y,z,α,β,θ)；
步骤s77，根据相机的外参及相机相对二维码的位姿camPos(x,y,z,α,β,θ)推算出机器人相对二维码的位姿btPos(x,y,z,α,β,θ)；
步骤s78，通过二维码全局坐标barP(x,y,z,α,β,γ)及机器人相对二维码位姿btPos(x,y,z,α,β,θ)，推算出机器人的在地图中的全局位姿bPos(x,y,z,α,β,θ)＝barP(x,y,z,α,β,θ)+btPos(x,y,z,α,β,θ)。
若当前帧检测到二维码，则通过卡尔曼滤波器融合二维码定位和“视觉-惯导”定位，实现全局位姿校正，包括，
首先，求解二维码定位的噪声方差：
图5为以QR码为例的二维码的距离图像中心偏移量示意图，
的计算公式为：
其中w,h分别为图像的宽和高,dx,dy分别为二维码中心到图像中心的距离在x轴和y轴上的投影。通过增加位置偏差噪声
二维码检测的噪声建模更符合实际噪声来源。
其次，求解“视觉-惯导”定位噪声方差：
ds为相邻两次检测到二维码的时间间隔内机器人走过的路程,“惯导-视觉”里程计定位的测量噪声方差为ds的函数。
如果当前周期内没有检测到二维码，则机器人位置就是“视觉-惯导”里程计定位的输出值，即机器人当前位置Pos(k)＝vPos(k)。
如果当前周期检测到二维码，则根据卡尔曼滤波器融合机器人当前位置，即
本发明的基于多传感器的机器人定位方法即应用，融合了环境纹理、二维码、编码器和惯性测量单元的信息，提高了定位的精度和鲁棒性。通过“视觉-惯导”里程计技术，能够提供每帧的视觉和惯导的定位融合，实现了对视觉信息的更有效应用。在没有二维码的区域利用“视觉-惯导”里程计提供有效的定位信息，在检测到二维码的时候，二维码提供机器人全局位姿信息，校正“视觉-惯导”里程计的累积误差。而在融合策略方面，编码器和IMU的增量融合方式，更符合积分定位方式的模型，实现了更精确的航迹推演定位。惯导航迹和单目视觉的融合，不仅能够恢复单目视觉的尺度信息，也能抑制位姿角的漂移。
Claims (7)
1.一种基于多传感器的地面机器人定位方法，其特征在于，所述定位方法包括：
步骤s1，在机器人上安装相机和IMU惯性测量单元，在驱动轮上安装编码器，并标定传感器的内参和外参；
步骤s2，在地面上铺设二维码，并将二维码的全局坐标和尺寸信息进行编码；
步骤s3，机器人的初始位置放置在某个二维码上方，通过二维码定位来初始化机器人的位置；
步骤s4，在机器人行进过程中，采集图像数据、IMU数据和编码器数据，不同传感器具有不同的采样周期；
步骤s5，编码器里程计模块通过卡尔曼滤波器，增量融合IMU和编码器数据，进行航迹推演，推算出机器人连续三维位姿轨迹gPk(x，y，θ)；
步骤s6，“视觉-惯导”定位模块结合图像、IMU、编码器里程计轨迹及运动流形约束；通过非线性优化技术求解出机器人的当前六维位姿vPosk(x，y，z，α，β，θ)；
步骤s7，二维码检测模块扫描图像，如果能检测出二维码，则根据二维码特征点求解出机器人的全局位姿bPosk(x，y，z，α，β，θ)，否则bPosk(x，y，z，α，β，θ)设为无效值；
步骤s8：利用卡尔曼滤波器融合“视觉-惯导”定位vPosk(x，y，z，α，β，θ)及二维码定位bPosk(x，y，z，α，β，θ)的值，融合后的位姿作为机器人的当前位姿。
2.根据权利要求1所述的方法，其特征在于，对机器人硬件的处理有，
标定机器人驱动轮直径，建立编码器读数增量到车轮移动距离的变换函数Tr(x)；
标定机器人左右驱动轮的轮间距B；
标定相机内参和外参；
标定IMU的外参；
对多个传感器需要进行时间戳对齐。
3.根据权利要求1所述的方法，其特征在于，
在步骤s5中，编码器里程计模块通过卡尔曼滤波器融合编码器和IMU数据，融合策略采用增量融合的方式；增量融合策略是指仅融合传感器的增量值，而不是绝对数值；
假设时刻tk-1的机器人编码器里程计求解的三维位姿为gPk-1＝(x(k-1)，y(k-1)，θ(k-1))；
卡尔曼滤波器增量融合的步骤为：
步骤s51，从IMU的陀螺仪来估计机器人方向角变化量，
陀螺仪提供角速度信息，通过对yaw方向角速度的积分，可以得到机器人相对上次定位时刻航向角变化量为：
其中ng(k)为陀螺仪角度测量的高斯噪声，该噪声的均值为零，方差为
随着时间线性增长；
步骤s52，从编码器来估计机器人航向角变化量，
根据编码器读数增量及变换函数Tr(x)，计算出左轮行进距离ΔSl和右轮行驶距离ΔSr，则机器人走过的路程为ΔS＝(ΔSl+ΔSr)*0.5；
机器人的航向角变化量为：
e(dθ)＝(ΔSr-ΔSl)/B+ne(k)
其中ne(k)为编码器测量的高斯噪声，该噪声的均值为零，方差为
随着机器人行走路程的增加线性增长；B为机器人左右轮间距；
步骤s53，融合航向角变化量，
增量融合g(dθ)和e(dθ)，融合结果作为编码器里程计的航向角变化量，根据卡尔曼滤波器原理，有：
方差σg(k)和σe(k)的值都是从上个采样时刻起开始估算，
而Δθ的方差为
在机器人静止和运动时，系数c1和c2是变化的；
步骤s54，推算机器人在二维平面上的位置变化量，
在每个很短的采样周期内，机器人的行走的路程非常小，因此可以将机器人的轨迹看作多个微小的直线段组成；则机器人在x，y坐标轴上的位置变化可以表示为：
Δx＝ΔS*cos(θ(k-1)+Δθ/2)
Δy＝ΔS*sin(θ(k-1)+Δθ/2)
步骤s55，航迹推演得到机器人在平面上的三维位姿，
x(k)＝x(k-1)+Δx
y(k)＝y(k-1)+Δy
θ(k)＝θ(k-1)+Δθ
航迹推演获取机器人在地平面上的连续轨迹。
4.根据权利要求1所述的方法，其特征在于，在步骤s6中，“视觉-惯导”定位模块包括，
步骤s61，读取相机图像、IMU和编码器里程计轨迹数据，视觉跟踪模块检测并跟踪图像特征Features；预积分模块对IMU数据进行预积分处理；
步骤s62，参数初始化，获取尺度、重力、速度以及IMU bias信息；
步骤s63，使用滑动窗来维护最近的数据，滑动窗的数据包含有图像帧及其对应的图像特征features、IMU惯性测量数据及编码器里程计轨迹数据；滑动窗的长度是固定的，通过边缘化(marginalization)操作删除较旧或者不符合要求的数据，加入最新的数据；
步骤s64：通过非线性优化技术计算机器人的最优位姿vPosk(x，y，z，α，β，θ)，使其最符合滑动窗内的图像、IMU、编码器里程计数据以及机器人运动流形约束。
5.根据权利要求1所述的方法，其特征在于，
机器人通过二维码定位来初始化位置，如果机器人开机后检测不到二维码，则机器人会向服务器发送“位置初始化失败”信息，并通过机器人上的声光报警装置来报警显示。
6.根据权利要求1所述的方法，其特征在于，在步骤s7中，二维码检测算法具有以下步骤：
步骤s71，对整幅图像做滤波及增强对比度处理；
步骤s72，利用相机内参，对图像做畸变校正处理；
步骤s73，对图像做二值化处理，生成二值图像；
步骤s74，扫描二值图像上，检测二维码；如果没有检测到二维码，则退出检测模块；如果检测到二维码，则继续下面的步骤；
步骤s75，将二维码图像区域变换为标准二维码图案，并根据二维码编码规则解码二维码图案，获取二维码的坐标信息barP(x，y，z，α，β，θ)；
步骤s76，检测二维码特征点，通过计算机视觉的pnp技术求解出相机相对二维码的位姿camPos(x，y，z，α，β，θ)；
步骤s77，根据相机的外参及相机相对二维码的位姿camPos(x，y，z，α，β，θ)推算出机器人相对二维码的位姿btPos(x，y，z，α，β，θ)；
步骤s78，通过二维码坐标位置barP(x，y，z，α，β，θ)及机器人相对二维码位姿btPos(x，y，z，α，β，θ)，推算出机器人的在地图中的全局位姿bPos(x，y，z，α，β，θ)＝barP(x，y，z，α，β，θ)+btPos(x，y，z，α，β，θ)。
7.根据权利要求1所述的方法，其特征在于，在步骤s8中，通过卡尔曼滤波器融合二维码定位和“视觉-惯导”定位，实现全局位姿校正，包括，
首先，求解二维码定位的噪声方差：
其次，求解“视觉-惯导”定位噪声方差：
ds为相邻两次检测到二维码的时间间隔内机器人走过的路程；
如果当前周期内没有检测到二维码，则机器人位置就是“视觉-惯导”定位的输出值，即机器人当前位置Pos(k)＝vPos(k)，
如果当前周期检测到二维码，则根据卡尔曼滤波器融合机器人当前位置，即
二维码定位提供了全局位姿校正功能。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010544006.0A CN113804184A (zh) | 2020-06-15 | 2020-06-15 | 基于多传感器的地面机器人定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010544006.0A CN113804184A (zh) | 2020-06-15 | 2020-06-15 | 基于多传感器的地面机器人定位方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113804184A true CN113804184A (zh) | 2021-12-17 |
Family
ID=78944088
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010544006.0A Pending CN113804184A (zh) | 2020-06-15 | 2020-06-15 | 基于多传感器的地面机器人定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113804184A (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114440874A (zh) * | 2021-12-31 | 2022-05-06 | 深圳市云鼠科技开发有限公司 | 基于光流和光栅的融合定位方法和装置 |
| CN114578820A (zh) * | 2022-03-02 | 2022-06-03 | 上海核工程研究设计院有限公司 | 一种核电站检测爬壁机器人的行进轨迹控制方法 |
| CN115079703A (zh) * | 2022-07-22 | 2022-09-20 | 安徽工业大学 | 外卖配送机器人及控制方法 |
| CN115859212A (zh) * | 2022-11-17 | 2023-03-28 | 广东智能无人系统研究院 | 一种用于海洋设备的自主布放回收的方法及系统 |
| CN116185009A (zh) * | 2022-12-29 | 2023-05-30 | 无锡弘宜智能科技股份有限公司 | 基于复合导航的差速agv纠偏控制系统及方法 |
| WO2024001649A1 (zh) * | 2022-06-29 | 2024-01-04 | 深圳市海柔创新科技有限公司 | 机器人定位方法、装置和计算可读存储介质 |
-
2020
- 2020-06-15 CN CN202010544006.0A patent/CN113804184A/zh active Pending
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114440874A (zh) * | 2021-12-31 | 2022-05-06 | 深圳市云鼠科技开发有限公司 | 基于光流和光栅的融合定位方法和装置 |
| CN114578820A (zh) * | 2022-03-02 | 2022-06-03 | 上海核工程研究设计院有限公司 | 一种核电站检测爬壁机器人的行进轨迹控制方法 |
| CN114578820B (zh) * | 2022-03-02 | 2024-05-07 | 上海核工程研究设计院股份有限公司 | 一种核电站检测爬壁机器人的行进轨迹控制方法 |
| WO2024001649A1 (zh) * | 2022-06-29 | 2024-01-04 | 深圳市海柔创新科技有限公司 | 机器人定位方法、装置和计算可读存储介质 |
| CN115079703A (zh) * | 2022-07-22 | 2022-09-20 | 安徽工业大学 | 外卖配送机器人及控制方法 |
| CN115859212A (zh) * | 2022-11-17 | 2023-03-28 | 广东智能无人系统研究院 | 一种用于海洋设备的自主布放回收的方法及系统 |
| CN116185009A (zh) * | 2022-12-29 | 2023-05-30 | 无锡弘宜智能科技股份有限公司 | 基于复合导航的差速agv纠偏控制系统及方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113804184A (zh) | 基于多传感器的地面机器人定位方法 | |
| CN108827315B (zh) | 基于流形预积分的视觉惯性里程计位姿估计方法及装置 | |
| CN106840148B (zh) | 室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法 | |
| US10133278B2 (en) | Apparatus of controlling movement of mobile robot mounted with wide angle camera and method thereof | |
| US20230194306A1 (en) | Multi-sensor fusion-based slam method and system | |
| Scaramuzza et al. | Visual odometry [tutorial] | |
| US10275649B2 (en) | Apparatus of recognizing position of mobile robot using direct tracking and method thereof | |
| US10399228B2 (en) | Apparatus for recognizing position of mobile robot using edge based refinement and method thereof | |
| Su et al. | GR-LOAM: LiDAR-based sensor fusion SLAM for ground robots on complex terrain | |
| CN107909614B (zh) | 一种gps失效环境下巡检机器人定位方法 | |
| CN112197770B (zh) | 一种机器人的定位方法及其定位装置 | |
| EP3159122A1 (en) | Device and method for recognizing location of mobile robot by means of search-based correlation matching | |
| CN113052908B (zh) | 一种基于多传感器数据融合的移动机器人位姿估计算法 | |
| CN114018248B (zh) | 一种融合码盘和激光雷达的里程计方法与建图方法 | |
| Niu et al. | IC-GVINS: A robust, real-time, INS-centric GNSS-visual-inertial navigation system | |
| CN111932674A (zh) | 一种线激光视觉惯性系统的优化方法 | |
| CN112254729A (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
| CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| Lin et al. | A sparse visual odometry technique based on pose adjustment with keyframe matching | |
| CN112179373A (zh) | 一种视觉里程计的测量方法及视觉里程计 | |
| Jiang et al. | Panoramic visual-inertial SLAM tightly coupled with a wheel encoder | |
| Khoshelham et al. | Vehicle positioning in the absence of GNSS signals: Potential of visual-inertial odometry | |
| Le et al. | Vehicle localization using omnidirectional camera with GPS supporting in wide urban area | |
| Pöppl et al. | Integrated trajectory estimation for 3D kinematic mapping with GNSS, INS and imaging sensors: A framework and review | |
| Sorensen et al. | On-line optical flow feedback for mobile robot localization/navigation |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| WD01 | Invention patent application deemed withdrawn after publication | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20211217 |