CN109358624A - 用于机器人的耦合定位方法 - Google Patents

Abstract

本发明公开了一种用于机器人的耦合定位方法，包括获取编码器左右轮速度并记录硬件时间戳；获取深度数据和彩色数据并记录获硬件时间戳；对机器人的编码器和深度摄像头进行软件同步；设定机器人的初始位置，初始化采集图，计算路标的3D点坐标和初始化定位系统；采集摄像头的彩色数据和深度数据并进行特征提取；采集编码器数据并得到均值和协方差；进行g2o优化得到机器人的位置信息；检测回环并对位置信息进行优化得到最优的定位数据；存储定位数据并进行下一轮定位。本发明保证了在视觉在丢位置的情况下的精准定位，而且在图优化过程中能够有效避免丢帧情况下还能继续采用编码器进行定位，保证了里程计的连续性。

Description

用于机器人的耦合定位方法

技术领域

本发明具体涉及一种用于机器人的耦合定位方法。

背景技术

随着经济技术的发展和人们生活水平的提高，机器人技术已经逐步深入人们的生产和生活中，给人们的生产和生活带来了无尽的便利。

目前，随着机器人技术的不断进步，对机器人的要求也越来越高，其中包括机器人的精确定位问题。目前市场上的机器人定位主要借助于各种传感器，包括激光雷达，码盘，惯性设备，视觉等等。在实际应用中，单一的传感器定位各有其优缺点，很难兼顾各种恶劣场景及环境，因此，基于传感器的融合成为了定位的趋势。

目前，利用视觉进行定位发展迅速，出现了基于单目的，双目的，深度的视觉里程计算法，分别是LSD_SLAM(Large-Scale Direct Monocular SLAM)，ORB_SLAM，RTABMAP(Real-Time Appearance-Based Mapping)，RGBDSALM，VO(Visual Odometry)等等。在测试中，单纯的VO算法并不能保证里程计的连续，当出现纹理不丰富，光照条件不好时，里程计经常丢失。所以，为了解决单纯视觉容易丢失的情况，因此需结合其它传感器进行弥补，从而保证视觉里程计的完整与准确。另外，从价格上考虑，编码器因为价格低廉，且相对惯导设备来说，避免了零偏的影响，所以充分利用编码器和视觉的优点，可以很好的完成机器人的定位。

但是，目前常用的基于机器视觉和编码器的定位方法，其依旧存在如定位成本高昂，定位精度相对较差等问题，从而也制约了机器人的定位技术的发展。

发明内容

本发明的目的在于提供一种定位精度高且成本相对低廉的用于机器人的耦合定位方法。

本发明提供的这种用于机器人的耦合定位方法，包括如下步骤：

S1.获取机器人的编码器左右轮速度，并同时记录获取的速度的硬件时间戳；

S2.获取机器人深度摄像头的深度数据和彩色数据，并同时记录获取数据的硬件时间戳；

S3.根据步骤S1和S2获取的硬件时间戳，对机器人的编码器和深度摄像头进行软件同步，同时初始化机器人的状态；

S4.设定机器人的初始位置，并通过深度摄像头初始化采集图，并利用世界地图坐标系与图像坐标系间的转换关系计算路标的3D点坐标，初始化定位系统；

S5.采集摄像头的彩色数据和深度数据，并分别进行特征提取；

S6.采集编码器的数据，并进行预积分计算从而得到均值和协方差；

S7.通过步骤S5和步骤S6得到的数据进行g2o优化，从而得到机器人的位置信息；

S8.检测回环，并对机器人的位置信息进行优化得到最优的定位数据；

S9.存储机器人的定位数据，并进行下一轮的定位。

步骤S3所述的进行软件同步，具体为保证机器人深度摄像头数据和编码器的测量单元的初始时间一致。

步骤S6所述的进行预积分计算从而得到均值和协方差，具体为采用如下步骤进行计算：

A.采用如下算式计算机器人的双轮差动模型的线速度vf和角速度w：

式中vl和vr为机器人编码器反馈的机器人左轮和右轮的速度；l为轮间距；

B.采用如下算式计算机器人的运动角度和半径，且得到的机器人的运动角度和半径即为所述的均值：

θ＝∑Δθ

式中r1和r2为机器人的两个轮胎的运动半径；θ为机器人的运动角度；vf为线速度；w为角速度；Δθ为机器人在编码器的相邻两次数据采集时间内所走过的角度，且Δθ＝w*Δt，Δt为编码器的相邻两次数据采集时间的时间间隔；

C.采用如下算式计算协方差：

式中cov_k+1为k+1时刻的协方差，I为单位矩阵，Δt为编码器的相邻两次数据采集时间的时间间隔， 且R＝-Δ_R×skew(vf)×Δt，dR＝Expmap(w×Δt)，RT＝Δ_R×Δt，Δ_R为两次获取数据时的归一化旋转矩阵，Expmap为李群SO(3)上的指数映射。

本发明提供的这种用于机器人的耦合定位方法，通过松紧耦合的方式将编码器与深度视觉进行融合，保证了在视觉在丢位置的情况下的精准定位，弥补了深度视觉的定位缺陷，而且在图优化过程中提出了协方差计算的方法，能够有效避免丢帧情况下还能继续采用编码器进行定位，从而保证了里程计的连续性。

附图说明

图1为本发明方法的方法流程图。

图2为本发明方法中的检测回环示意图。

具体实施方式

如图1所示为本发明方法的方法流程图：本发明提供的这种用于机器人的耦合定位方法，包括如下步骤：

S1.获取机器人的编码器左右轮速度，并同时记录获取的速度的硬件时间戳；

S2.获取机器人深度摄像头的深度数据和彩色数据，并同时记录获取数据的硬件时间戳；

S3.根据步骤S1和S2获取的硬件时间戳，对机器人的编码器和深度摄像头进行软件同步，同时初始化机器人的状态；所述的进行软件同步，具体为保证机器人深度摄像头数据和编码器的测量单元的初始时间一致；

S4.设定机器人的初始位置，并通过深度摄像头初始化采集图，并利用世界地图坐标系与图像坐标系间的转换关系计算路标的3D点坐标，初始化定位系统；

S5.采集摄像头的彩色数据和深度数据，并分别进行特征提取；

S6.采集编码器的数据，并进行预积分计算从而得到均值和协方差；具体为采用如下步骤进行计算：

A.采用如下算式计算机器人的双轮差动模型的线速度vf和角速度w：

式中vl和vr为机器人编码器反馈的机器人左轮和右轮的速度；l为轮间距；

B.采用如下算式计算机器人的运动角度和半径，且得到的机器人的运动角度和半径即为所述的均值：

θ＝∑Δθ

式中r1和r2为机器人的两个轮胎的运动半径；θ为机器人的运动角度；vf为线速度；w为角速度；Δθ为机器人在编码器的相邻两次数据采集时间内所走过的角度，且Δθ＝w*Δt，Δt为编码器的相邻两次数据采集时间的时间间隔；

C.采用如下算式计算协方差：

式中cov_k+1为k+1时刻的协方差，I为单位矩阵，Δt为编码器的相邻两次数据采集时间的时间间隔， 且R＝-Δ_R×skew(vf)×Δt，dR＝Expmap(w×Δt)，RT＝Δ_R×Δt，Δ_R为两次获取数据时的归一化旋转矩阵，Expmap为李群SO(3)上的指数映射；

S7.通过步骤S5和步骤S6得到的数据进行g2o优化，从而得到机器人的位置信息；

S8.检测回环，并对机器人的位置信息进行优化得到最优的定位数据；具体为检测回环，并对回环的机器人位姿采用g2o进行优化，对位姿和姿态的累积误差进行矫正，得到最优的定位数据；

如图2所示即为本发明方法中的位姿图：其中，图中部的小方块是路标在图像上的特征点位置，图中上部的圆形(T1～T3)是两相邻位姿之间的里程计测量值。机器人位姿x和路标点y作为位姿图的节点，位姿与位姿之间通过传感器测量值进行约束，构成图中节点之间的边。位姿图中的节点变量如下：

x＝{x₀,x₁,...,x_n,y₁,y₂,...,y_m}

一般情况下，传感器的测量与节点之间的关系通用方程如下：

z＝f(x)+v

其中，通过传感器测量获得数据称为观测数据z，待估计的变量为状态x，观测噪声v服从高斯分布。根据位姿图中的节点和边约束优化位姿的问题转化为求解最大似然估计：

x^*＝argmaxp(x|z)

更具体的，本系统对应的观测量z有两种，一种是图像中的二维特征点位置，即世界坐标三维点P_W和图像平面中的投影位置p，满足如下关系：

式中k为相机内参，I为单位矩阵，T_BC是相机与机器人坐标系之间的外参，T_BW为待估计的位姿，P_W为待估计的特征点位置在世界坐标系中的三维坐标，p为三维点在图像平面中的投影位置p，nl服从高斯分布

另一种是编码器提供的相对位姿，满足如下关系：

上式中，和为两个相邻的图像帧中对应的位姿。其中，噪声n_ij∈R⁶，Exp(n_ij)是从R6到SE(3)的指数映射；

当该位姿图中的节点数量超过预设计的滑动窗口大小时，将该位姿图内最旧的节点及约束边删除，并将删除的边约束作为先验约束加入该位姿图。同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，得到机器人的位姿信息，并将该机器人的位姿信息作为该机器人的定位结果输出；

S9.存储机器人的定位数据，并进行下一轮的定位。

Claims (3)

1.一种用于机器人的耦合定位方法，包括如下步骤：

S1.获取机器人的编码器左右轮速度，并同时记录获取的速度的硬件时间戳；

S2.获取机器人深度摄像头的深度数据和彩色数据，并同时记录获取数据的硬件时间戳；

S3.根据步骤S1和S2获取的硬件时间戳，对机器人的编码器和深度摄像头进行软件同步，同时初始化机器人的状态；

S4.设定机器人的初始位置，并通过深度摄像头初始化采集图，并利用世界地图坐标系与图像坐标系间的转换关系计算路标的3D点坐标，初始化定位系统；

S5.采集摄像头的彩色数据和深度数据，并分别进行特征提取；

S6.采集编码器的数据，并进行预积分计算从而得到均值和协方差；

S7.通过步骤S5和步骤S6得到的数据进行g2o优化，从而得到机器人的位置信息；

S8.检测回环，并对机器人的位置信息进行优化得到最优的定位数据；

S9.存储机器人的定位数据，并进行下一轮的定位。

2.根据权利要求1所述的用于机器人的耦合定位方法，其特征在于步骤S3所述的进行软件同步，具体为保证机器人深度摄像头数据和编码器的测量单元的初始时间一致。

3.根据权利要求2所述的用于机器人的耦合定位方法，其特征在于步骤S6所述的进行预积分计算从而得到均值和协方差，具体为采用如下步骤进行计算：

A.采用如下算式计算机器人的双轮差动模型的线速度vf和角速度w：

式中vl和vr为机器人编码器反馈的机器人左轮和右轮的速度；l为轮间距；

B.采用如下算式计算机器人的运动角度和半径，且得到的机器人的运动角度和半径即为所述的均值：

θ＝∑Δθ

式中r1和r2为机器人的两个轮胎的运动半径；θ为机器人的运动角度；vf为线速度；w为角速度；Δθ为机器人在编码器的相邻两次数据采集时间内所走过的角度，且Δθ＝w*Δt，Δt为编码器的相邻两次数据采集时间的时间间隔；

C.采用如下算式计算协方差：

式中cov_k+1为k+1时刻的协方差，I为单位矩阵，Δt为编码器的相邻两次数据采集时间的时间间隔， 且R＝-Δ_R×skew(vf)×Δt，dR＝Expmap(w×Δt)，RT＝Δ_R×Δt，Δ_R为两次获取数据时的归一化旋转矩阵，Expmap为李群SO(3)上的指数映射。