CN106525053A - 一种基于多传感器融合的移动机器人室内定位方法 - Google Patents

Abstract

本发明提供了一种基于多传感器融合的移动机器人室内定位方法，使用基于航迹推算的里程计位置数据补偿激光匹配定位中无法识别相似环境的情况。本发明不仅将基于惯性单元和里程计的传感器定位数据进行融合，也将激光雷达匹配定位作为参考指标，不仅减小了里程计的累积误差，还弥补了短距离激光雷达在单一特征环境中无法进行匹配定位的缺陷，为机器人的地图绘制和导航提供更为准确的定位数据。

Description

一种基于多传感器融合的移动机器人室内定位方法

技术领域

本发明属于轮式移动机器人室内定位领域，尤其是涉及一种基于多传感器融合的移动机器人室内定位方法。

背景技术

移动机器人在室内环境中移动时，首先要知道自己处于什么位置，这是机器人进行环境地图绘制、自主导航的重要前提就是精确的位置和位姿。因此，室内移动机器人定位技术一直是机器人领域的研究热点和难点。

随着传感技术的进步，用于室内移动机器人定位的传感器也在不断更新，总体来说，当前的机器人定位方法主要分为两类：相对定位法和绝对定位法。相对定位法指机器人根据自身传感器，如里程计、惯性模块等得到短时间内的相对位移及转向，结合上一采样时刻的累加位姿得出机器人在当前时刻的位姿，主要有航迹推算法和惯性导航法。

航迹推算法主要用于短距离定位，长距离时编码器的累积误差十分明显；加速度计和陀螺仪存在***误差和零漂、温漂等问题，也会造成累积误差。

绝对定位法指机器人依靠外部传感器直接确定其在世界坐标系中的位姿，常使用路标法、GPS、地图匹配等。路标法维护困难，并且需要对环境进行改变；GPS多用于室外，不适合在室内环境使用；地图匹配常用激光雷达获取环境信息，将前后采用数据进行匹配以获得机器人全局位姿，但大量程的激光雷达造价昂贵，小量程的激光雷达无法处理像楼道一般环境特征相似，变化不明显的场景。

在以往的研究中，有人提出每隔一段时间采用激光扫描匹配校准里程计定位。但这往往需要使用价格昂贵的大量程激光雷达，并且在一些特征单一的室内环境中，激光匹配无法根据匹配结果推断位移，因此利用其定位数据校准里程计会造成数据错误，因此，本发明提出一种基于多传感器融合的移动机器人室内定位方法，使用基于航迹推算的里程计位置数据补偿激光匹配定位中无法识别相似环境的情况，在提高室内定位精度的基础上，其环境适应性更强。

发明内容

有鉴于此，本发明旨在提出一种基于多传感器融合的移动机器人室内定位方法，为机器人的地图绘制和导航提供更为准确的定位数据。

为达到上述目的，本发明的技术方案是这样实现的：

一种基于多传感器融合的移动机器人室内定位方法，利用激光数据匹配的ICP算法进行定位估算，使用航迹推算法得到的位姿变化进行定位结果补偿。

进一步的，所述的利用激光数据匹配的ICP算法进行定位估算包括：

设采样周期为Δt，在第i个采样周期之前，机器人的准确位姿为

p＝(x,y,θ)^T

在第i个采样周期，通过激光扫描匹配得到机器人移动的位姿变化为：

Δp_{icp_i}＝[Δx_{ipc_i} Δy_{ipc_i} Δθ_{ipc_i}]^T；

设此时机器人的移动速度为v_{icp_i}＝[v_{icp_xi},v_{icp_yi},w_{icp_i}]^T，因此存在以下关系：

v_{icp_xi}＝Δx_{icp_i}/Δt或v_{icp_xi}＝Δy_{icp_i}/Δt，并且ΔS_{icp_i}＝Δx_{icp_i}或ΔS_{icp_i}＝Δy_{icp_i}，

其中ΔS_{icp_i}为通过匹配算法得到的机器人在本采样周期内的移动距离。

进一步的，利用激光数据匹配的ICP算法进行定位估算具体包括：

(a1)将本时刻激光扫描记为当前扫描D，上一时刻扫描记为参考扫描M；

(b1)通过点到线的匹配方法得到将D匹配到M的最优变换(R,T)，其中R为旋转变换矩阵，T为平移矢量：

(c1)根据(R,T)计算当前机器人的位姿变化Δp_k＝(Δx_k,Δy_k,Δθ_k)^T，假定k时刻机器人位姿为p_k＝(x_k,y_k,θ_k)^T，则k+1时刻机器人位姿为：

(d1)将当前扫描D记为新的参考扫描M，继续采样激光数据，由步骤(a1)开始重新迭代。

进一步的，所述的使用航迹推算法得到的位姿变化进行定位结果补偿包括：

使用航迹推算法进行定位估算，得到第i个采样周期内机器人移动的位姿变化为：

Δp_{track_i}＝[Δx_{track_i},Δy_{track_i},Δθ_{track_i}]^T；

设此时机器人的移动速度为v_{track_i}＝[v_{track_xi},v_{track_yi},w_{track_i}]^T，则：

v_{track_xi}＝ΔS_{track_i}/Δt，v_{track_yi}＝0.0，w_i＝Δθ_{track_i}/Δt；

其中ΔS_{track_i}为通过航迹推算法得到的机器人在本采样周期内的移动距离。

进一步的，所述的使用航迹推算法得到的位姿变化进行定位结果补偿具体包括：

(a2)假设在一个采样周期内，收到电机编码器反馈，单位换算得到左轮、右轮的移动距离分别为ΔS_l和ΔS_r，转过的角度为Δθ，则可以推算得到机器人在这个采用周期内的移动距离和旋转角角度：

其中，2R为机器人两轮间距，ΔS为机器人移动的距离，Δθ为机器人转过的角度；

(b2)机器人在世界坐标系下的运动可如下式所示：

其中，θ为此时刻之前机器人已旋转过的累积角度；

(c2)由此，假定k时刻机器人位姿为p_k＝(x_k,y_k,θ_k)，单位采样时间内机器人位姿变化为Δp_k＝(Δx_k,Δy_k,Δθ_k)，则根据航迹推算法，机器人在k+1时刻的位姿为：

进一步的，还包括利用激光数据匹配的ICP算法进行定位估算得到的位姿变化以及使用航迹推算法得到的位姿变化做差，得到位姿差Δp。

进一步的，若满足

Δp≤ξ，其中ξ>0，

则认为此时的激光扫描匹配得到的位姿变化正确

进一步的，若不满足

Δp≤ξ，其中ξ>0，

则启用航迹推算法得到的位姿变化进行定位结果补偿。

相对于现有技术，本发明所述的一种基于多传感器融合的移动机器人室内定位方法具有以下优势：

(1)本发明将基于激光雷达的环境匹配定位算法与基于里程计的航迹推算法结合，完成移动机器人的室内定位；

(2)本发明采用基于航迹推算的里程计数据补偿激光雷达定位数据，解决了使用短量程激光雷达无法区分特征单一且长时间无变化的环境。

附图说明

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明实施例所述的一种基于多传感器融合的移动机器人室内定位方法示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

下面将参考附图并结合实施例来详细说明本发明。

本发明使用基于航迹推算的里程计定位数据补偿激光雷达的扫描匹配定位，该方法的实现基于如下理论：

(1)基于编码器的航迹推算模型

假设在一个采样周期内，收到电机编码器反馈，单位换算得到左轮、右轮的移动距离分别为ΔS_l和ΔS_r，转过的角度为Δθ，则可以推算得到机器人在这个采用周期内的移动距离和旋转角角度：

其中，2R为机器人两轮间距，ΔS为机器人移动的距离，Δθ为机器人转过的角度。

由于采样间隔十分短，可以认为机器人位移近似于直线，则机器人在世界坐标系下的运动可如下式所示：

其中，θ为此时刻之前机器人已旋转过的累积角度。

由此，假定k时刻机器人位姿为p_k＝(x_k,y_k,θ_k)，单位采样时间内机器人位姿变化为Δp_k＝(Δx_k,Δy_k,Δθ_k)，则根据航迹推算法，机器人在k+1时刻的位姿为：

(2)基于激光数据匹配的ICP算法

ICP算法的思想是对连续采集的相邻两个激光数据帧进行匹配，获取两者之间的相对位姿变换关系，从而增量式地更新机器人当前位姿，算法步骤可概括为：

(a)将本时刻激光扫描记为当前扫描D，上一时刻扫描记为参考扫描M；

(b)通过点到线的匹配方法得到将D匹配到M的最优变换(R,T)，其中R为旋转变换矩阵，T为平移矢量：

(c)根据(R,T)计算当前机器人的位姿变化Δp_k＝(Δx_k,Δy_k,Δθ_k)^T，假定k时刻机器人位姿为p_k＝(x_k,y_k,θ_k)^T，则k+1时刻机器人位姿为：

(d)将当前扫描D记为新的参考扫描M，继续采样激光数据，由步骤(a)开始重新迭代。

如图1所示，本发明所述的多传感器融合定位的算法步骤：

未知环境中，手动控制机器人行走建立环境地图过程中，只做直线前进和原地旋转动作，这一约定的目的是减小地图绘制过程中因误差而引起的地图倾斜。

假设采样周期为Δt，在第i个采样周期之前，机器人的准确位姿为p＝(x,y,θ)^T。

第一步，采用关键理论(2)中提到的基于激光数据匹配的ICP算法进行定位估算，在第i个采样周期，通过激光扫描匹配得到机器人移动的位姿变化为：Δp_{icp_i}＝[Δx_{ipc_i}Δy_{ipc_i} Δθ_{ipc_i}]^T

设此时机器人的移动速度为v_{icp_i}＝[v_{icp_xi},v_{icp_yi},w_{icp_i}]^T，对于双轮驱动移动移动机器人，v_y＝0恒成立。根据前提约定，只存在向前直行和原地转弯，因此以下等式恒成立：

v_{icp_xi}＝Δx_{icp_i}/Δt或v_{icp_xi}＝Δy_{icp_i}/Δt，并且ΔS_{icp_i}＝Δx_{icp_i}或ΔS_{icp_i}＝Δy_{icp_i}。

其中ΔS_{icp_i}为通过匹配算法得到的机器人在本采样周期内的移动距离。

第二步，使用航迹推算法进行定位估算，根据式(1)～(3)得到第i个采样周期内机器人移动的位姿变化为：

Δp_{track_i}＝[Δx_{track_i},Δy_{track_i},Δθ_{track_i}]^T

设此时机器人的移动速度为v_{track_i}＝[v_{track_xi},v_{track_yi},w_{track_i}]^T，则：

v_{track_xi}＝ΔS_{track_i}/Δt，v_{track_yi}＝0.0，w_i＝Δθ_{track_i}/Δt。

其中ΔS_{track_i}为通过航迹推算法得到的机器人在本采样周期内的移动距离。

第三步，验证激光扫描匹配得到的位姿变化是否正确：

第一、二步均是对第i个采样周期内机器人移动的位姿变化进行估算，若两者的估算均正确，应有：

|Δp_{track_i}-Δp_{icp_i}|≤ξ (5)

其中ξ是一个大于零的数，其含义为两种位姿估算法得到的位姿的最大允许误差。

若结果满足(5)式，则认为此时的激光扫描匹配得到的位姿变化正确，则该采样时间后，机器人位姿p_i可以表示为：

p_i＝p_i-1+Δp_{icp_i} (6)

若计算结果不满足(5)式，进一步判断，如果此时v_{icp_i}≈0.0但v_{track_i}≠0.0，则判断此时的激光扫描匹配得到的位姿变化错误，猜想机器人进入结构特征单一路段，启用航迹推算法得到的位姿变化进行定位结果补偿，即：

p_i＝p_i-1+Δp_{track_i} (7)

如此往复，直至机器人创建封闭的环境地图为止。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于多传感器融合的移动机器人室内定位方法，其特征在于：利用激光数据匹配的ICP算法进行定位估算，使用航迹推算法得到的位姿变化进行定位结果补偿。

2.根据权利要求1所述的一种基于多传感器融合的移动机器人室内定位方法，其特征在于：所述的利用激光数据匹配的ICP算法进行定位估算包括：

设采样周期为Δt，在第i个采样周期之前，机器人的准确位姿为

p＝(x,y,θ)^T

在第i个采样周期，通过激光扫描匹配得到机器人移动的位姿变化为：

Δp_{icp_i}＝[Δx_{ipc_i} Δy_{ipc_i} Δθ_{ipc_i}]^T；

设此时机器人的移动速度为v_{icp_i}＝[v_{icp_xi},v_{icp_yi},w_{icp_i}]^T，因此存在以下关系：

v_{icp_xi}＝Δx_{icp_i}/Δt或v_{icp_xi}＝Δy_{icp_i}/Δt，并且ΔS_{icp_i}＝Δx_{icp_i}或ΔS_{icp_i}＝Δy_{icp_i}，

其中ΔS_{icp_i}为通过匹配算法得到的机器人在本采样周期内的移动距离。

3.根据权利要求2所述的一种基于多传感器融合的移动机器人室内定位方法，其特征在于：利用激光数据匹配的ICP算法进行定位估算具体包括：

(a1)将本时刻激光扫描记为当前扫描D，上一时刻扫描记为参考扫描M；

(b1)通过点到线的匹配方法得到将D匹配到M的最优变换(R,T)，其中R为旋转变换矩阵，T为平移矢量：

(c1)根据(R,T)计算当前机器人的位姿变化Δp_k＝(Δx_k,Δy_k,Δθ_k)^T，假定k时刻机器人位姿为p_k＝(x_k,y_k,θ_k)^T，则k+1时刻机器人位姿为：

${(x_{k+1},y_{k+1},{\mathrm{\θ}}_{k+1})}^T=\left[\begin{array}{c}{x}_k\\ y_k\\ {\mathrm{\θ}}_k\end{array}\right]+\left[\begin{array}{ccc}{\mathrm{cos\θ}}_k& {\mathrm{sin\θ}}_k& 0\\ -{\mathrm{sin\θ}}_k& {\mathrm{cos\θ}}_k& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\Δx}}_k\\ {\mathrm{\Δy}}_k\\ {\mathrm{\Δ\θ}}_k\end{array}\right]$

(d1)将当前扫描D记为新的参考扫描M，继续采样激光数据，由步骤(a1)开始重新迭代。

4.根据权利要求1所述的一种基于多传感器融合的移动机器人室内定位方法，其特征在于：所述的使用航迹推算法得到的位姿变化进行定位结果补偿包括：

使用航迹推算法进行定位估算，得到第i个采样周期内机器人移动的位姿变化为：

Δp_{track_i}＝[Δx_{track_i},Δy_{track_i},Δθ_{track_i}]^T；

设此时机器人的移动速度为v_{track_i}＝[v_{track_xi},v_{track_yi},w_{track_i}]^T，则：

v_{track_xi}＝ΔS_{track_i}/Δt，v_{track_yi}＝0.0，w_i＝Δθ_{track_i}/Δt；

其中ΔS_{track_i}为通过航迹推算法得到的机器人在本采样周期内的移动距离。

5.根据权利要求4所述的一种基于多传感器融合的移动机器人室内定位方法，其特征在于：所述的使用航迹推算法得到的位姿变化进行定位结果补偿具体包括：

(a2)假设在一个采样周期内，收到电机编码器反馈，单位换算得到左轮、右轮的移动距离分别为ΔS_l和ΔS_r，转过的角度为Δθ，则可以推算得到机器人在这个采用周期内的移动距离和旋转角角度：

$\left\{\begin{array}{c}\mathrm{\Δ}S=\frac{{\mathrm{\ΔS}}_r+{\mathrm{\ΔS}}_l}{2}\\ \mathrm{\Δ}\mathrm{\θ}=\frac{{\mathrm{\ΔS}}_r-{\mathrm{\ΔS}}_l}{2R}\end{array}\right.$

其中，2R为机器人两轮间距，ΔS为机器人移动的距离，Δθ为机器人转过的角度；

(b2)机器人在世界坐标系下的运动可如下式所示：

$\left\{\begin{array}{c}\mathrm{\Δ}x=\mathrm{\Δ}Scos(\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ}/2)\\ \mathrm{\Δ}y=\mathrm{\Δ}Ssin(\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ}/2)\end{array}\right.$

其中，θ为此时刻之前机器人已旋转过的累积角度；

(c2)由此，假定k时刻机器人位姿为p_k＝(x_k,y_k,θ_k)，单位采样时间内机器人位姿变化为Δp_k＝(Δx_k,Δy_k,Δθ_k)，则根据航迹推算法，机器人在k+1时刻的位姿为：

$p_{k+1}=\left[\begin{array}{c}{x}_k+{\mathrm{\Δx}}_k\\ y_k+{\mathrm{\Δy}}_k\\ {\mathrm{\θ}}_k+\mathrm{\Δ}\mathrm{\θ}\end{array}\right].$

6.根据权利要求1所述的一种基于多传感器融合的移动机器人室内定位方法，其特征在于：还包括利用激光数据匹配的ICP算法进行定位估算得到的位姿变化以及使用航迹推算法得到的位姿变化做差，得到位姿差Δp。

7.根据权利要求6所述的一种基于多传感器融合的移动机器人室内定位方法，其特征在于：若满足

Δp≤ξ，其中ξ>0，

则认为此时的激光扫描匹配得到的位姿变化正确。

8.根据权利要求6所述的一种基于多传感器融合的移动机器人室内定位方法，其特征在于：

若不满足

Δp≤ξ，其中ξ>0，

则启用航迹推算法得到的位姿变化进行定位结果补偿。