一种基于SLAM导航移动机器人的全局定位方法

摘要

本发明公开了一种基于SLAM导航移动机器人的全局定位方法，属于移动机器人自动导航技术领域。为解决现有技术中实现移动机器人全局定位需要对应用环境进行改造，增加辅助定位的特征物体，或者需要在移动机器人设备上安装辅助设备等。这些方案本身不能适应较为复杂环境、成本较高、定位精度较差、极为不方便，因此定位精度难以有更大的优化与提高的问题，本方法包括以下4个步骤：步骤1移动机器人应用环境的子区域的选取，步骤2数据点的采集，步骤3子区域选取合理与否的分析判断，步骤4基于ICP实现移动机器人全局定位。用于基于激光SLAM导航移动机器人尤其是AGV(自动导航小车)复杂环境下的全局定位。

主权项

一种基于SLAM导航移动机器人的全局定位方法，其特征在于，所述方法包括以下步骤：步骤1 移动机器人应用环境的子区域的选取通过移动机器人自带的传感器，获取室内环境的数据信息，并通过SLAM技术，构建关于室内环境完整的平面地图，设整个应用环境为Σ，在该环境下选取多个矩形的子区域，取地图中位姿为零点所对应的环境区域为Σ₀，其它的子区域分别为Σ₁,Σ₂,…,Σ_n；假设移动机器人人工移动最大距离不超过D，那么任意相邻的两个子区域的几何中心的距离都不应该超过2D，且应该满足2D>l，l表示子区域沿移动机器人运行方向的长度；步骤2 数据点的采集步骤1中在移动机器人应用环境中选取矩形的子区域，它的宽度和长度分别设为d、l，宽度d表示子区域沿垂直于移动机器人运行方向的宽度，长度l表示子区域沿移动机器人运行方向的长度，那么每个子区域宽度与长度分别表示为d₁,d₂,…,d_n，l₁,l₂,…,l_n，将第i个子区域分别沿宽度与长度方向上分别等距分割为m_i、k_i等份，得到m_i*k_i个矩形，将移动机器人放在分割得到的每个小矩形的几何中心，可获取沿前进或后退方向上移动机器人自带传感器扫描数据Q_i，Q_i包含m_i*k_i组匹配数据，并保存在全局定位数据库中，每个子区域获取的数据都存在一个到移动机器人在构建地图中位姿的一一映射ψ:Q_it→(x_it,y_it,θ_it)(1≤i≤n,1≤t≤m_i*k_i)；设用于Delaunay迭代最近点算法匹配的函数为L:(P,Q)→(R,T)，该函数的输入为待匹配的两个点集，定义P为目标点集，Q为模型点集，输出为两个点集进行匹配的旋转矩阵R与平移向量T，旋转矩阵R(θ)表示沿逆时针方向旋转θ(θ>0)，是2×2的矩阵，而平移向量T表示2×1的列向量，并定义每个点集中点的个数为N(Q_i)；步骤3 子区域选取合理与否的分析判断步骤3A 设第i个子区域，第r组匹配数据，表示为Q_ir，初始值i＝1，r＝1，P表示数据库中任意一组模型点集，匹配误差ε_ir表示与第i个子区域中第r组数据匹配的误差，ε_ir计算方法为：其中p_j、q_j分别表示点集P、Q_ir中的任一点坐标；步骤3B 将Q_ir与数据库中已选取的所有子区域内的所有组数据进行匹配，获取对应的匹配误差如下：${ϵ}_s^1=\left\{{ϵ}_{s1},{ϵ}_{s2},...,{ϵ}_{s(m_s*k_s)}\right\},(s=i)$步骤3C 当中的任何一个元素的值都小于中任何一个元素值，r＝r+1，再返回步骤3B，否则退出循环，重新从应用环境中选取子区域Σ_i，当r＝m_i*k_i+1时，执行步骤3D；步骤3D 改变变量i＝i+1，r＝1，然后返回步骤3B，继续执行，直至i＝n+1，退出循环；步骤4 基于ICP实现移动机器人全局定位步骤4A 将移动机器人置于应用环境中已选取的任一子区域Σ_i内，任何位置获取一组传感器原始数据P，数据Q_i表示在第i个子区域Σ_i采集的数据，设在定位程序启动前移动机器人在地图中的位姿为(x₀,y₀,θ₀)，定义初始变量c＝0,0<δ<π/6，定义初始总误差E_ic＝0，i＝1；步骤4B 先将数据P进行处理得P_h＝R(cδ)*P，然后通过定义的ICP匹配函数L，与全局定位数据库中的数据Q_i进行匹配得到m_i*k_i组变换矩阵(R_t,T_t)＝L(P_h,Q_it)(1≤t≤m_i*k_i),其中Q_it表示第i个子区域采集的第t组匹配数据；步骤4C 根据每组变换矩阵先计算变换后的点集可表示为P_t＝R_t*P_h+T_t，然后计算每组匹配数据的误差为其中p_r、q_r分别表示点集P_t、Q_it中的任一点，在子区域Σ_i内将所有组数据匹配一次得到总的匹配误差可表示为$E_{ic}=\sqrt{\left({\Sigma }_{t=1}^{m_i*k_i}{{ϵ}_t}^2\right)/\left(m_i*k_i\right)};$步骤4D 将子区域Σ_i的数据库与激光传感器扫描的数据P在2π弧度内进行多次匹配，改变参数的值c＝c+1，当满足时继续执行步骤4E，否则返回步骤4B，其中Γ(x)(x>0)表示取不超过x的最大整数；步骤4E 改变全局定位数据库中匹配点集的子区域，i＝i+1，当i＝n时继续执行步骤4F，否则返回步骤4A；步骤4F 共获得多组总的匹配误差取误差中最小的两组E_min1与E_min2作为移动机器人全局定位的数据，设此时对应的min1＝(x₁,y₁)，mim2＝(x₂,y₂)，由于移动机器人在全局中的位姿只能在某一个子区域内，因此，该结果必须同时满足条件x₁＝x₂＝x，其中l(x₁)与d(x₁)分别表示第x₁子区间所分割的小矩形的长度与宽度，当满足条件时继续执行步骤4G，否则让移动机器人自动向前或者向后移动一定的距离，再返回步骤4A重新执行；步骤4G 根据上述结果确定移动机器人在全局地图中的位置，根据第x个子区域内采集的m_xk_x组点集Q_x与P_h(c＝y₁∪y₂)之间匹配可得到m_xk_x组误差，将其按照从小到大顺序排列，分别取前三个最小的误差，设结果分别为${ϵ}_{{\mathrm{xy}}_1}=\{{ϵ}_{y_{1}1},{ϵ}_{y_{1}2},{ϵ}_{y_{1}3}\},$${ϵ}_{{\mathrm{xy}}_2}=\{{ϵ}_{y_{2}1},{ϵ}_{y_{2}2},{ϵ}_{y_{2}3}\},$其对应的Q_x中的六组匹配点集可设为步骤4H 步骤4G中共得到的六个误差，同样按照从小到大的顺序排列，并取前三个最小的误差，设得到对应误差为{ε₁′,ε₂′,ε₃′}，对应的点集可分别设为{Q_x1′,Q_x2′,Q_x2′}，根据映射关系ψ，设三个点集对应的移动机器人在地图中的位姿分别为步骤4I 设定方差阈值σ，计算三个误差的方差σ₂，如果满足σ₃<σ，则继续执行步骤4J，否则去掉三个误差中最大的那个，然后再计算两个误差的方差σ₁，如果满足σ₁<σ，则继续执行步骤4K，否则去掉两个误差中最大的那个，然后继续执行步骤4L；步骤4J 假设最终有三个误差{ε₁′,ε₂′,ε₃′}，每个误差对应的匹配变换矩阵分别为(x₀,y₀,θ₀)表示移动机器人丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为：$\left[\begin{array}{c}{\theta }_{01}\\ {\theta }_{02}\\ {\theta }_{03}\end{array}\right]={\theta }_0+\delta \left[\begin{array}{c}{c}_1\\ c_2\\ c_3\end{array}\right]$其中x_y与y₀不变，然后根据匹配变换矩阵计算移动机器人三个全局地图的位姿可表示如下；[θ₁ θ₂ θ₃]^T＝[θ₀₁ θ₀₂ θ₀₃]^T+[β₁ β₂ β₃]^T$\left[\begin{array}{cc}{x}_1& y_1\\ x_2& y_2\\ x_3& y_3\end{array}\right]=\left[\begin{array}{cc}{x}_0& y_0\\ x_0& y_0\\ x_0& y_0\end{array}\right]+\left[\begin{array}{cc}{T}_1\left(1\right)& T_1\left(2\right)\\ T_2\left(1\right)& T_2\left(2\right)\\ T_3\left(1\right)& T_3\left(2\right)\end{array}\right]$计算三个权重其中i＝1,2,3，根据权重可最终确定移动机器人全局定位的位姿可表示如下：$\left[\begin{array}{c}x\\ y\\ \theta \end{array}\right]=\left[\begin{array}{ccc}{x}_1& x_2& x_3\\ y_1& y_2& y_3\\ {\theta }_1& {\theta }_2& {\theta }_3\end{array}\right]\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\end{array}\right]$步骤4K 假设最终有两个误差{ε₁′,ε₂′,}，每个误差对应的匹配变换矩阵分别为(x₀,y₀,θ₀)表示移动机器人丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为：$\left[\begin{array}{c}{\theta }_{01}\\ {\theta }_{02}\end{array}\right]={\theta }_0+\delta \left[\begin{array}{c}{c}_1\\ c_2\end{array}\right]$其中x₀与y₀不变，然后根据匹配变换矩阵计算移动机器人两个全局地图的位姿可表示如下；[θ₁ θ₂]^T＝[θ₀₁ θ₀₂]^T+[β₁ β₂]^T$\left[\begin{array}{cc}{x}_1& y_1\\ x_2& y_2\end{array}\right]=\left[\begin{array}{cc}{x}_0& y_0\\ x_0& y_0\end{array}\right]+\left[\begin{array}{cc}{T}_1\left(1\right)& T_1\left(2\right)\\ T_2\left(1\right)& T_2\left(2\right)\end{array}\right]$计算两个权重其中i＝1,2，根据权重可最终确定移动机器人全局定位的位姿可表示如下：$\left[\begin{array}{c}x\\ y\\ \theta \end{array}\right]=\left[\begin{array}{cc}{x}_1& x_2\\ y_1& y_2\\ {\theta }_1& {\theta }_2\end{array}\right]\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\end{array}\right]$步骤4L 假设最只有误差ε₁′,，对应的匹配变换矩阵分别为(R(β_l),T_l),(x₀,y₀,θ₀)表示移动机器人丢失位姿后保存的位姿，首先根据R(cδ)变换其位姿为：θ₀₁＝θ₀+δc₁其中x₀与y₀不变，然后根据匹配变换矩阵计算移动机器人全局地图的位姿可表示如下；θ₁＝θ₀₁+β₁$\left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]=\left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]+\left[\begin{array}{c}{T}_1\left(1\right)\\ T_1\left(2\right)\end{array}\right]$计算权重ω₁＝1，根据权重可最终确定移动机器人全局定位的位姿可表示如下：$\left[\begin{array}{c}x\\ y\\ \theta \end{array}\right]={\omega }_1\left[\begin{array}{c}{x}_1\\ y_1\\ {\theta }_1\end{array}\right].$