基于多传感器的地面机器人定位方法与流程

1.本发明涉及智能机器人领域，特别涉及一种基于多传感器的地面机器人定位方法。


背景技术：

2.机器人定位是指机器人在移动过程中，通过采集的传感器数据，与地图进行信息匹配，从而获取自身在环境中的位姿。定位是机器人基本的模块，准确的自我定位是机器人导航的前提。
3.二维码定位是常见的一种定位方式，在仓储机器人、分拣机器人等方向应用广泛。预先在地面上铺设二维码，机器人在二维码组成的网格上运动；通过相机拍摄地面，识别图像中的二维码来实现机器人全局定位。两个二维码之间的路程通过imu惯性测量单元和编码器来定位，从而给导航算法提供连续的轨迹信息。但是由于地面起伏、轮胎侧滑等原因的影响，imu和编码器定位误差会迅速积累。从而导致机器人的轨迹误差和停止误差增大。
4.场景中的二维码是离散铺设的，在机器人行进过程中，能够检测到二维码的图像比例非常低，往往远低于1％。在没有铺设二维码的区域，相机拍摄的图像数据没能提供定位信息，这就造成了视觉信息的浪费。
5.如何在不增加成本的前提下，提高机器人上现有的传感器数据的利用率，改善定位精度和鲁棒性，是亟需解决的问题。
[0006]“视觉-惯导”里程计(visual-inertial odometry)技术通过融合单目相机和imu惯性测量单元数据，能够提供真实尺度的定位信息。在无二维码的区域，可以使用“视觉-惯导”里程计技术来提供定位信息，提高视觉信息的利用率，并与编码器里程计的定位结果融合互补。
[0007]
但是，“视觉-惯导”里程计因imu的积分受到其初始值的影响，在地面机器人上的定位效果不好。地面机器人的运动往往比较平稳，多以匀加速直线运动、原地旋转运动为主，这种运动会导致“视觉-惯导”里程计的尺度信息无法观测。地面机器人的pitch和roll方向的变化非常小，也很难实现精确观测。机器人yaw方向角度也会随着时间漂移。
[0008]
如何抑制“视觉-惯导”里程计在地面机器人应用中遇到的上述问题，是影响“惯导-视觉”里程计定位精度的关键问题。
[0009]
在多传感器定位时，如何评估不同传感器的定位误差，根据何种策略来融合数据，都是影响融合效果的重要因素。标准的卡尔曼滤波器的融合策略并不适用所有情形，需要在实践中探索新的融合方案。


技术实现要素：

[0010]
鉴于上述技术中的不足，本发明提供了新的多传感器定位方法，该在不改变硬件配置的情况下，更有效的利用传感器数据，提高地面机器人定位精度及鲁棒性。
[0011]
第一方面，通过“视觉-惯导”里程计技术使得每帧图像都能提供定位信息。当图像
中存在二维码时，通过二维码检测算法提供全局位置信息校；当图像中不存在二维码时，则通过“视觉-惯导”里程计算法来提供连续定位。
[0012]
第二方面，改进了“视觉-惯导”里程计技术，把编码器里程计的航迹数据和机器人运动流形约束加入到非线性优化中。编码器里程计轨迹能够提供真实尺度信息，而运动流形约束则对机器人的pitch和roll角度提供约束。从而消除了地面机器人的“视觉-惯导”里程计中尺度、pitch和roll这三个维度难以测量的问题。
[0013]
第三方面，改进了多传感器定位融合策略。编码器和imu数据融合策略采用融合增量值的方式，而不是融合绝对数值；增量值及其噪声方差仅对最近时间段的运动状态进行建模，建模精度更高。二维码定位噪声方差加入了位置相关项，二维码中心距离图像中心越远，定位噪声方差越大；该方差项体现了相机的成像特性，即离图像中心越远，相机的投影畸变和成像噪声越大。
[0014]
基于多传感器的定位方法，包含以下步骤：
[0015]
步骤s1，在机器人上安装相机和imu惯性测量单元，在驱动轮上安装编码器，并标定传感器的内参和外参；
[0016]
步骤s2，在地面上铺设二维码，并将二维码的全局坐标和尺寸信息进行编码；
[0017]
步骤s3，机器人的初始位置放置在某个二维码上方，通过二维码定位来初始化机器人的位置；
[0018]
步骤s4，在机器人行进过程中，采集图像数据、imu数据和编码器数据，不同传感器具有不同的采样周期；
[0019]
步骤s5，编码器里程计模块通过卡尔曼滤波器，增量融合imu和编码器数据，进行航迹推演，推算出机器人连续三维位姿轨迹gp
k
(x,y,θ)；
[0020]
步骤s6，“视觉-惯导”里程计定位模块结合图像、imu、编码器里程计轨迹及运动流形约束；通过非线性优化技术求解出机器人的当前六维位姿vpos
k
(x,y,z,α,β,θ)；
[0021]
步骤s7，二维码检测模块扫描图像，如果能检测出二维码，则根据二维码特征点求解出机器人的全局位姿bpos
k
(x,y,z,α,β,θ)，否则bpos
k
(x,y,z,α,β,θ)设为无效值；
[0022]
步骤s8，利用卡尔曼滤波器融合“视觉-惯导”定位vpos
k
(x,y,z,α,β,θ)及二维码定位bpos
k
(x,y,z,α,β,θ)的值。融合后的位姿作为机器人的当前位姿。
[0023]
多传感器定位方法，在硬件方面的操作有：标定机器人驱动轮直径，建立编码器读数增量到车轮移动距离的变换函数tr(x)；标定机器人左右驱动轮的轮间距b；标定相机内参和外参；标定imu的外参；对多个传感器需进行时间戳对齐。
[0024]
编码器里程计的增量融合方案为：
[0025]
假设时刻t
k-1
的机器人编码器里程计求解的三维位姿为gp
k-1
＝(x(k-1),y(k-1),θ(k-1))；
[0026]
步骤s51，从imu的陀螺仪来估计机器人方向角变化量,
[0027]
陀螺仪提供角速度信息，通过对yaw方向角速度的积分，可以得到机器人相对上次定位时刻航向角变化量为：
[0028][0029]
其中n
g
(k)为陀螺仪角度测量的高斯噪声，该噪声的均值为零，方差为
随着时间线性增长；
[0030]
步骤s52，从编码器来估计机器人航向角变化量，
[0031]
根据编码器读数增量及变换函数tr(x)，计算左轮行进距离δs
l
和右轮行驶距离δs
r
，则机器人走过的路程为δs＝(δs
l
δs
r
)*0.5；
[0032]
机器人的航向角变化量为：e(dθ)＝(δs
r-δs
l
)/b n
e
(k)
[0033]
其中n
e
(k)为编码器测量的高斯噪声，该噪声的均值为零，方差为随着机器人行走路程的增加线性增长；b为机器人左右轮间距；
[0034]
步骤s53，融合航向角变化量，
[0035]
增量融合g(dθ)和e(dθ)，融合结果作为编码器里程计的航向角变化量；根据卡尔曼滤波器原理，有：
[0036][0037]
方差σ
g
(k)和σ
e
(k)的值都是从上个采样时刻起开始估算。
[0038]
而δθ的方差为在机器人静止和运动时，系数c1和c2是变化的；
[0039]
步骤s54，推算机器人在二维平面上的位置变化量，
[0040]
在每个很短的采样周期内，机器人的行走的路程非常小，因此可以将机器人的轨迹看作多个微小的直线段组成。则机器人在x,y坐标轴上的位置变化可以表示为：
[0041]
δx＝δs*cos(θ(k-1) δθ/2)
[0042]
δy＝δs*sin(θ(k-1) δθ/2)
[0043]
步骤s55，航迹推演得到机器人在平面上的三维位姿，
[0044]
x(k)＝x(k-1) δx
[0045]
y(k)＝y(k-1) δy
[0046]
θ(k)＝θ(k-1) δθ
[0047]
在步骤s6中，“视觉-惯导”里程计定位的步骤为：
[0048]
步骤s61，读取相机图像、imu和编码器里程计轨迹数据，视觉跟踪模块检测并跟踪图像特征features；预积分模块对imu数据进行预积分处理；
[0049]
步骤s62，参数初始化，获取尺度、重力、速度以及imu bias信息；
[0050]
步骤s63，使用滑动窗来维护最近的数据，滑动窗的数据包含有图像帧及其对应的图像特征features、imu惯性测量数据及编码器里程计轨迹数据；滑动窗的长度是固定的，通过边缘化(marginalization)操作删除较旧或者不符合要求的数据，加入最新的数据；
[0051]
步骤s64：通过非线性优化技术计算机器人的最优位姿vpos
k
(x,y,z,α,β,θ)，使其最符合滑动窗内的图像、imu、编码器里程计数据以及机器人运动流形约束。
[0052]
机器人通过二维码定位来初始化位置，如果机器人开机后检测不到二维码，则机器人会向服务器发送“位置初始化失败”信息，并通过机器人上的声光报警装置来报警显示。
[0053]
在步骤s7中，二维码检测算法的具体步骤为：
[0054]
步骤s71，对整幅图像做滤波及增强对比度处理；
[0055]
步骤s72，利用相机内参，对图像做畸变校正处理；
[0056]
步骤s73，对图像做二值化处理，生成二值图像；
[0057]
步骤s74，扫描二值图像上，检测二维码；如果没有检测到二维码，则退出检测模块；如果检测到二维码，则继续下面的步骤；
[0058]
步骤s75，将二维码图像区域变换为标准二维码图案，并根据二维码编码规则解码二维码图案，获取二维码的坐标信息barp(x,y,z,α,β,θ)；
[0059]
步骤s76，检测二维码特征点，通过计算机视觉的pnp技术求解出相机相对二维码的位姿campos(x,y,z,α,β,θ)；
[0060]
步骤s77，根据相机的外参及相机相对二维码的位姿campos(x,y,z,α,β,θ)推算出机器人相对二维码的位姿btpos(x,y,z,α,β,θ)；
[0061]
步骤s78，通过二维码坐标位置barp(x,y,z,α,β,θ)及机器人相对二维码位姿btpos(x,y,z,α,β,θ)，推算出机器人的在地图中的全局位姿bpos(x,y,z,α,β,θ)＝barp(x,y,z,α,β,θ) btpos(x,y,z,α,β,θ)。
[0062]
若当前帧检测到二维码，则通过卡尔曼滤波器融合二维码定位和“视觉-惯导”里程计定位，实现全局位姿校正，包括，
[0063]
首先，求解二维码定位的噪声方差：
[0064]
其中为固定噪声方差；为二维码在图像中的成像位置引起的噪声方差；虽然图像预处理会做畸变校正，但是靠近图像边缘的位置成像噪声比较大，纹理压缩严重；二维码中心与图像中心距离越大，检测误差越大；
[0065]
其次，求解“视觉-惯导”定位的噪声方差：
[0066]
ds为相邻两次检测到二维码的时间间隔内机器人走过的路程；
[0067]
如果当前周期内没有检测到二维码，则机器人位置就是“视觉-惯导”里程计的输出值，即机器人当前位置pos(k)＝vpos(k)；
[0068]
如果当前周期检测到二维码，则根据卡尔曼滤波器融合机器人当前位置，即
[0069][0070]
本发明的有益效果如下：
[0071]
1).通过“视觉-惯导”里程计技术，能够提供逐帧的视觉和惯导的定位融合，实现了对视觉信息的更有效应用。在没有二维码的区域利用“视觉-惯导”定位，在检测到二维码的时候，二维码提供机器人全局位姿信息，校正“视觉-惯导”定位的累积误差。
[0072]
2).将编码器里程计和运动流形约束添加到“视觉-惯导”里程计技术中，提供了尺度信息及pitch、roll方向的约束，使得“视觉-惯导”里程计技术更适用地面机器人。
[0073]
3).采用增量融合方式来融合编码器和imu数据，更符合积分定位方式的模型，实现了更精确的航迹推演定位
[0074]
4).本发明的多传感器机器人定位方法，融合了环境纹理、二维码、编码器和惯性测量单元的信息，实现了传感器数据的更高效利用，提高了定位的精度和鲁棒性。
附图说明
[0075]
图1是整个多传感器定位算法的流程图。
[0076]
图2是机器人由k-1时刻到k时刻的运动轨迹示意图。
[0077]
图3为机器人位姿变换示意图。
[0078]
图4为“视觉-惯导”里程计定位流程图。
[0079]
图5示出了以qr码为例的二维码中心到图像中心的距离。
具体实施方式
[0080]
下面参照附图来详细描述本发明的具体实施，但本实施例并不用于限制本发明，凡是采用本发明的相似结构及相似变化，均应列入本发明的保护范围。
[0081]
多传感器定位方法，在硬件方面的操作有：标定机器人驱动轮直径，建立编码器读数增量到车轮移动距离的变换函数tr(x)；标定机器人左右驱动轮的轮间距b；标定相机内参和外参；标定imu的外参；对多个传感器需进行时间戳对齐。
[0082]
图1示出了多传感器定位系统的组成及处理流程。主要步骤有：
[0083]
步骤s1，在机器人上安装相机和imu惯性测量单元，在驱动轮上安装编码器，并标定传感器的内参和外参；
[0084]
步骤s2，在地面上铺设二维码，并将二维码的全局坐标和尺寸信息进行编码；
[0085]
步骤s3，机器人的初始位置放置在某个二维码上方，通过二维码定位来初始化机器人的位置；
[0086]
步骤s4，在机器人行进过程中，采集图像数据、imu数据和编码器数据，不同传感器具有不同的采样周期；
[0087]
步骤s5，编码器里程计模块通过卡尔曼滤波器，增量融合imu和编码器数据，进行航迹推演，推算出机器人连续三维位姿轨迹gp
k
(x,y,θ)；
[0088]
步骤s6，“视觉-惯导”里程计定位模块结合图像、imu、编码器里程计轨迹及运动流形约束；通过非线性优化技术求解出机器人的当前六维位姿vpos
k
(x,y,z,α,β,θ)；
[0089]
步骤s7，二维码检测模块扫描图像，如果能检测出二维码，则根据二维码特征点求解出机器人的全局位姿bpos
k
(x,y,z,α,β,θ)，否则bpos
k
(x,y,z,α,β,θ)设为无效值；
[0090]
步骤s8，利用卡尔曼滤波器融合“视觉-惯导”定位vpos
k
(x,y,z,α,β,θ)及二维码定位bpos
k
(x,y,z,α,β,θ)的值。融合后的位姿作为机器人的当前位姿。
[0091]
机器人通过二维码定位来初始化位置，如果机器人开机后检测不到二维码，则机器人会向服务器发送“位置初始化失败”信息，并通过机器人上的声光报警装置来报警显示。
[0092]
图2示出了机器人从时刻k-1到时刻k的运动轨迹。在很短的时间间隔内，机器人的左右轮的运动速度认为匀速不变，则机器人的线速度和角速度保持不变。机器人看作沿着一个圆弧运动，该圆弧对应的圆心角等于机器人方向角的变化量δθ。由于δθ的值很小，因此机器人走过的路程圆弧δs近似等于该圆弧对应的弦δd，即有δs＝δd。
[0093]
图3示出了机器人从时刻k-1到时刻k的位姿变化。步骤s5融合编码器和imu传感器增量数据通过航迹推演方法来推算该位姿变化信息，主要步骤如下：
[0094]
假设时刻t
k-1
的机器人编码器里程计求解的三维位姿为gp
k-1
＝(x(k-1),y(k-1),θ
(k-1))；
[0095]
步骤s51，从imu的陀螺仪来估计机器人航向角变化量，
[0096]
陀螺仪提供角速度信息，通过对yaw方向角速度的积分，可以得到机器人相对上次定位时刻方向角变化量为：
[0097][0098]
其中n
g
(k)为陀螺仪角度测量的高斯噪声，该噪声的均值为零，方差为随着时间线性增长。
[0099]
步骤s52，从编码器来估计机器人航向角变化，
[0100]
根据编码器读数增量及变换函数tr(x)，求解出左轮行进距离δs
l
及右轮行驶距离δs
r
，则机器人走过的路程为δs＝(δs
l
δs
r
)*0.5；
[0101]
机器人的方向角变化量为：
[0102]
e(dθ)＝(δs
r-δs
l
)/b n
e
(k)，
[0103]
其中n
e
(k)为编码器测量的高斯噪声，该噪声的方差为随着机器人行走路程的增加线性增长。b为机器人左右轮间距。
[0104]
步骤s53，融合航向角变化量，
[0105]
融合g(dθ)和e(dθ)，融合结果作为编码器里程计的方向角变化量。根据卡尔曼滤波器原理，有：
[0106][0107]
在机器人运动过程中，σ
g
(k)和σ
e
(k)都是一直增加的。一直增加的数值在应用中是难以处理的，因此σ
g
(k)和σ
e
(k)的值都是从上个采样时刻起开始估算。
[0108]
而δθ的方差为在机器人静止和运动时，系数c1和c2是变化的。
[0109]
步骤s54，推算机器人在二维平面上的位置变化，
[0110]
在每个很短的采样周期内，机器人的行走的路程非常小，因此可以将机器人的轨迹看作多个微小的直线段组成。则机器人在x,y坐标轴上的位置变化可以表示为：
[0111]
δx＝δs*cos(θ(k-1) δθ/2)
[0112]
δy＝δs*sin(θ(k-1) δθ/2)
[0113]
步骤s55，航迹推演得到机器人三维位姿：
[0114]
x(k)＝x(k-1) δx
[0115]
y(k)＝y(k-1) δy
[0116]
θ(k)＝θ(k-1) δθ
[0117]
图4示出了步骤s6中“视觉-惯导”定位的具体算法流程：
[0118]
步骤s61，预处理，
[0119]
获取相机、imu和编码器里程计轨迹数据。视觉跟踪模块检测并跟踪图像特征features；预积分模块对imu数据进行预积分处理；编码器里程计模块融合imu和编码器数据进行航迹推演；得到当前时刻的位置、速度和旋转姿态。
[0120]
步骤s62，初始化，
[0121]
初始化需要获取尺度、重力、速度以及imu bias信息。
[0122]
由于视觉structure-from-motion(sfm)在初始化的过程中表现更好，因此初始化首先进行视觉sfm，以此来估计相机位姿及特征点位置。为了限制计算量，可以仅使用最近的一定数量帧，即使用滑动窗(slide window)。视觉sfm的步骤为：(1)将第一帧作为关键帧；(2)检测最新帧的特征并与关键帧进行特征匹配；如果有足够多的匹配特征数量及足够的视差，则使用5点法来恢复两帧之间的旋转及有尺度自由度的平移；(3)三角化特征点；(4)通过pnp技术来解算和跟踪图像特征features，计算两帧之间的相对位姿并三角化feature points；(5)对滑动窗中所有帧及3d feature points进行最小化重投影误差的全局束调整优化，优化每帧的位姿及3d feature points。
[0123]
视觉sfm后，将imu的预积分结果与视觉sfm对齐，求解初始化参数。初始化的参数有：陀螺仪的偏移量、初始化速度、重力向量和尺度因子。
[0124]
步骤s63，数据滑动窗，
[0125]
为了限制计算量，一般使用滑动窗来维护最近的数据。滑动窗的数据包含有图像帧及其对应的图像特征features和imu惯性测量数据及编码器里程计数据。滑动窗的长度是固定的，通过边缘化(marginalization)操作删除较旧或者不符合要求的数据，加入最新的数据。
[0126]
步骤s64，非线性优化，
[0127]
建立“视觉-惯导”的集束调整方程c(x)，求解相机最优位姿解，使其最符合滑动窗内的图像、imu、编码器里程计数据以及机器人运动流形约束。
[0128]
运动流形约束可以表示为几何约束g(x)＝0。运动流形约束与地面地形有关；可选的，室内地面就可以认为是一个平面，那么机器人运动流形约束就是近似平面约束；在函数c(x)中加入项即实现了机器人运动的流形约束。
[0129]
获取相机位姿后，通过相机的外参就可以推导出机器人当前的位姿。
[0130]
在机器人行进过程中，二维码检测算法会对扫描图像进行二维码检测。二维码检测算法具有以下步骤：
[0131]
步骤s71，对整幅图像做滤波及增强对比度处理；
[0132]
步骤s72，利用相机内参，对图像做畸变校正处理；
[0133]
步骤s73，对图像做二值化处理，生成二值图像；
[0134]
步骤s74，扫描二值图像上，检测二维码；如果没有检测到二维码，则退出检测模块；如果检测到二维码，则继续下面的步骤；
[0135]
步骤s75，将二维码图像区域变换为标准二维码图案，并根据二维码编码规则解码二维码图案，获取二维码的全局坐标信息barp(x,y,z,α,β,θ)；
[0136]
步骤s76，检测二维码特征点，通过计算机视觉的pnp技术求解出相机相对二维码的位姿campos(x,y,z,α,β,θ)；
[0137]
步骤s77，根据相机的外参及相机相对二维码的位姿campos(x,y,z,α,β,θ)推算出机器人相对二维码的位姿btpos(x,y,z,α,β,θ)；
[0138]
步骤s78，通过二维码全局坐标barp(x,y,z,α,β,γ)及机器人相对二维码位姿btpos(x,y,z,α,β,θ)，推算出机器人的在地图中的全局位姿bpos(x,y,z,α,β,θ)＝barp(x,
y,z,α,β,θ) btpos(x,y,z,α,β,θ)。
[0139]
若当前帧检测到二维码，则通过卡尔曼滤波器融合二维码定位和“视觉-惯导”定位，实现全局位姿校正，包括，
[0140]
首先，求解二维码定位的噪声方差：
[0141]
其中为固定噪声方差；为二维码在图像中的成像位置引起的噪声方差。虽然图像预处理会做畸变校正，但是靠近图像边缘的位置成像噪声比较大，纹理压缩严重；二维码中心与图像中心距离越大，检测误差越大。
[0142]
图5为以qr码为例的二维码的距离图像中心偏移量示意图，的计算公式为：
[0143][0144]
其中w,h分别为图像的宽和高,dx,dy分别为二维码中心到图像中心的距离在x轴和y轴上的投影。通过增加位置偏差噪声二维码检测的噪声建模更符合实际噪声来源。
[0145]
其次，求解“视觉-惯导”定位噪声方差：
[0146]
ds为相邻两次检测到二维码的时间间隔内机器人走过的路程,“惯导-视觉”里程计定位的测量噪声方差为ds的函数。
[0147]
如果当前周期内没有检测到二维码，则机器人位置就是“视觉-惯导”里程计定位的输出值，即机器人当前位置pos(k)＝vpos(k)。
[0148]
如果当前周期检测到二维码，则根据卡尔曼滤波器融合机器人当前位置，即
[0149][0150]
本发明的基于多传感器的机器人定位方法即应用，融合了环境纹理、二维码、编码器和惯性测量单元的信息，提高了定位的精度和鲁棒性。通过“视觉-惯导”里程计技术，能够提供每帧的视觉和惯导的定位融合，实现了对视觉信息的更有效应用。在没有二维码的区域利用“视觉-惯导”里程计提供有效的定位信息，在检测到二维码的时候，二维码提供机器人全局位姿信息，校正“视觉-惯导”里程计的累积误差。而在融合策略方面，编码器和imu的增量融合方式，更符合积分定位方式的模型，实现了更精确的航迹推演定位。惯导航迹和单目视觉的融合，不仅能够恢复单目视觉的尺度信息，也能抑制位姿角的漂移。