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

技术特征：

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，

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