全向移动机器人通道内倒行导航方法与流程

本发明属于机器人运动规划导航技术领域，具体的为一种全向移动机器人通道内倒行导航方法，适用于仓库通道场景下根据环境信息导引机器人完成倒退行进。

背景技术：

全向移动机器人在平面空间上具有三个自由度，可同时进行前后、左右以及原地旋转三个维度上的运动，在不改变自身姿态的情况下向任意方向移动。由于机器人较高的机动性和灵活性，在精度要求较高的场景有着较为广泛的应用。特别是狭窄的仓库通道环境，是该机器人的典型工作场景。

仓库通道两侧摆放有货物，机器人需要沿通道行进，以完成仓库货物的定位、盘点等工作。而仓库货物堆积较多，导致仓库通道往往只有一个出入口，同时通道的宽度较窄，机器人无法在通道内完成转向，只能倒行退出通道。因此，通道内倒行导航技术符合实际需要也是必要的。

倒行导航与正向导航有着较大的区别，正向导航时，传感器安装在机器人前侧，传感器观测方向与机器人行进方向一致；而倒行导航恰恰相反，传感器观测方向与机器人行进方向恰恰相反。倒行导航通常需要正向导航的历史数据作为参考，与正向导航相比，倒行导航计算较为复杂，实现难度较大。

当前倒行导航的方案包括环境地图匹配导航、路标导航、里程计导航。在仓库通道环境下，由于环境特征不明显，thrun等人发现基于环境特征的地图匹配难以在这种环境下得到准确的结果。仓库的货物容易频繁变换，人工路标容易损坏，基于路标的导航不适用于该环境。邵帅等人发现里程计导航的累计误差随时间而增长，导航精度不能满足该场景要求。

在现有研究中，陈文学等人提出了环境地图匹配导航和里程计导航方法相结合的方法，使用里程计获得粗略位置信息可提升环境地图匹配的正确率，而使用环境地图匹配可减少里程计的累计误差。这种融合方法可以互相弥补两种导航方法各自的缺陷，得到较好的导航效果。但该方法仅考虑局部环境特征而非整个通道信息进行导航，这会导致行进路径的规划不合理，不适用于通道环境。

技术实现要素：

有鉴于此，本发明的目的在于提供一种全向移动机器人通道内倒行导航方法，可适用于仓库通道场景下根据环境信息导引机器人完成倒退行进，并具有易于实现、适应性强的优点。

为达到上述目的，本发明提供如下技术方案：

一种全向移动机器人通道内倒行导航方法，包括如下步骤：

步骤1：控制机器人在通道内完成向前行进，根据编码器估算机器人当前的置信息；

步骤2：将机器人行进过程中激光雷达获取到的实时数据生成为局部地图；

步骤3：根据小车实时位置以及获取实时局部地图，拼接局部地图，获得通道完整的全局地图；

步骤4：根据生成的全局地图，使用最小二乘法拟合通道两边边线，通过计算得到适合小车行进的轨迹线；

步骤5：机器人开始到行，激光雷达采集实时数据生成局部地图，将局部地图与全局地图进行匹配，获取小车的当前位置信息；

步骤6：根据机器人当前位置信息与轨迹线之间的偏差进行位姿调整，保持机器人跟随轨迹线。

进一步，所述步骤1中，记机器人几何中心到车轮的横向距离为a，纵向距离为b，机器人当前几何中心速度为vm，则得到小车逆向运动学模型：

其中，w为机器人当前角速度；vw1、vw2、vw3、vw4分别为机器人四个轮子的线速度；vwx、vwy分别为机器人几何中心速度vm在横向方向和纵向方向的速度分量；

根据逆向模型以及编码器数据计算机器人当前在水平方向和竖直方向的速度以及旋转速度，并通过积分得到当前位置信息：

机器人当前位置信息即为(xm,ym,wm)；

其中，vmxt为机器人当前在水平方向上的速度；vmyt为机器人当前在竖直方向上的速度；wmt为机器人当前的旋转速度。

进一步，所述步骤2中，生成局部地图的方法如下：

步骤21：获取每个点是否存在障碍物：

计算局部地图上的点与激光雷达的距离和角度信息，并根据角度信息来计算对应的激光雷达探测该方位的激光束，根据激光束所探测的距离与地图上的距离做比较来判断该点是否存在障碍物；

步骤22：更新局部地图

对于局部地图上的每个点，计算其是否存在障碍物。

进一步，所述步骤3中，将局部地图上的每个点，结合激光雷达当前位置信息变换到全局地图上，并更新全局地图信息。

进一步，所述步骤4中，将地图上标记为被占用的点分为左右两组，左右两组标记为被占用的点分别为通道的左边线和右边线，分别对两组标记为被占用的点的数据使用最小二乘法进行拟合；并取分别距左边线和右边线等距的点连成线，得到适合机器人行进的轨迹线。

进一步，所述步骤5中，局部地图与全局地图的匹配方法如下：

根据机器人编码器信息估计小车当前几何中心位置为(xm,ym,wm)，即得到激光雷达的位置为(xi,yi,wi)；

由于机器人实际位置与估计位置之间可能存在一定偏差，并记机器人实际偏角与估计偏角之间的偏差为σ，定义匹配度如下：

p＝π1-(gcicj-mij)

其中，gcicj表示全局地图上对应局部地图上的点存在障碍物的概率，mij表示局部地图上的点存在障碍物的概率；

结合小车当前可能存在的位置以及偏角，分别对局部地图进行旋转变换，得到对应的匹配度，取匹配度最高的情况，即为机器人当前的位姿，并根据激光雷达和小车几何中心的位置关系，得到小车几何中心的位置。

本发明的有益效果在于：

本发明的全向移动机器人通道内倒行导航方法在充分利用激光雷达采集的环境信息的情况下，结合全向移动机器人的特点，提出了基于激光雷达的全向移动机器人倒行导航方法，此方法为工程上实现全向移动机器人通道内倒行提供了一种解决方案，该方案易于实现、适应性强，能够解决通道内移动机器人倒行导航问题。

附图说明

为了使本发明的目的、技术方案和有益效果更加清楚，本发明提供如下附图进行说明：

图1为全向移动机器人的平面示意图；

图2为标记前局部栅格地图；

图3为标记后局部栅格地图；

图4为全局栅格地图拟合直线；

图5为全局栅格地图匹配；

图6为倒行导航示意图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好的理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

本实施例的全向移动机器人通道内倒行导航方法，包括如下步骤：

步骤1：控制机器人在通道内完成向前行进，根据编码器估算机器人当前的置信息。

如图1所示，m是小车的几何中心位置，记机器人几何中心到车轮的横向距离为a，纵向距离为b，机器人当前几何中心速度为vm，该速度在水平方向分量为vmx，竖直方向上的分量为vmy，机器人当前角速度为w，vw1、vw2、vw3、vw4分别表示图1中编号为1，2，3，4轮子的线速度。则得到小车逆向运动学模型：

根据逆向模型以及编码器数据计算机器人当前在水平方向和竖直方向的速度以及旋转速度，并通过积分得到当前位置信息：

机器人当前位置信息即为(xm,ym,wm)；

其中，vmxt为机器人当前在水平方向上的速度；vmyt为机器人当前在竖直方向上的速度；wmt为机器人当前的旋转速度。

步骤2：将机器人行进过程中激光雷达获取到的实时数据生成为局部地图。

记激光雷达扫描最大距离为zmax,激光雷达所处位置为(x,y)，局部地图上的点记为(xi,yi)，地图中(xi,yi)被占用记为locc，未占用记为lfree，不确定则记为l0，则生成局部地图的方法如下：

步骤21：获取每个点是否存在障碍物：

getmij(xi,yi,z)

if(zk＜zmaxand|r-zk|＜α/2)

return(locc)

elseif(r＜zk)

return(lfree)

else

return(l0)

其中xi、yi表示局部地图上的横纵坐标，z表示激光雷达所测数据集，x、y表示激光雷达在局部地图上的横纵坐标，r表示(x,y)与(xi,yi)之间的距离，表示坐标(xi,yi)对应角度值，φj表示第j个激光雷达数据对应的角度，k表示使得与φj偏差最小的j值，zk表示激光雷达第k个数据，zmax表示激光雷达测量的最大距离，α为激光雷达数据波动范围阈值，地图中该点有障碍物记为locc，没有则记为lfree，默认为l0。

计算局部地图上的点与激光雷达的距离和角度信息，并根据角度信息来计算对应的激光雷达探测该方位的激光束，根据激光束所探测的距离与地图上的距离做比较来判断该点是否存在障碍物。

步骤22：更新局部地图

foreach(mij:m)

mij＝getmij(xi,yi,z)

其中m表示局部地图，mij表示局部地图中横纵坐标分别为xi和yi的点，z表示激光雷达数据集。

对于局部地图上的每个点，计算其是否存在障碍物。

步骤3：根据小车实时位置以及获取实时局部地图，拼接局部地图，获得通道完整的全局地图。

将局部地图上的每个点，结合激光雷达当前位置信息变换到全局地图上，并更新全局地图信息。

记当前激光雷达在全局地图上坐标为(xi,yi,wi)，则有：

其中，r为前处理的地图位置与激光雷达所处位置之间的距离，xi、yi表示局部地图上的横纵坐标，x、y表示激光雷达在局部地图上的横纵坐标,表示坐标(xi,yi)对应角度值,xgi、ygi分别表示xi、yi对应于全局地图的坐标，wi表示小车当前的角度。

根据上述变换，将局部地图信息点转换到对应的全局地图，对全局地图进行更新。

步骤4：根据生成的全局地图，使用最小二乘法拟合通道两边边线，通过计算得到适合小车行进的轨迹线。

将地图上标记为被占用的点分为左右两组，左右两组标记为被占用的点分别为通道的左边线和右边线，分别对两组标记为被占用的点的数据使用最小二乘法进行拟合；并取分别距左边线和右边线等距的点连成线，得到适合机器人行进的轨迹线。

如图3所示，将地图上标记为locc的点分为两组，为通道的左边线和右边线，分别对两组数据使用最小二乘法进行拟合，拟合效果如图4中的位于两端的两条线所示。取距两条直线等距的点连成线，得到小车行进的最佳轨迹线，如图4中位于中间的线所示。

步骤5：机器人开始到行，激光雷达采集实时数据生成局部地图，将局部地图与全局地图进行匹配，获取小车的当前位置信息。

局部地图与全局地图的匹配方法如下：

根据机器人编码器信息估计小车当前几何中心位置为(xm,ym,wm)，即得到激光雷达的位置为(xi,yi,wi)；

根据小车编码器的信息估计小车当前几何中心位置为(xm,ym,wm)，由于激光雷达和小车的相对位置固定，即得到激光雷达的位置为(xi,yi,wi)，如图5中的深灰度色块所示，考虑到位置估计的不确定性，实际位置可能在图5中的淡灰度色块区域，并且角度可能存在一定偏差，记为σ。定义匹配度如下：

p＝π1-(gcicj-mij)

其中，mij表示局部地图上的点存在障碍物的概率，通过步骤21中getmij计算得到，gcicj表示全局地图上对应局部地图上的点存在障碍物的概率，即步骤3中得到全局地图上坐标为(xgi，ygi)的点。当该函数值越大时，匹配程度越高，即表示当前小车最有可能在的位置。确定小车位置时，结合小车当前可能存在的位置以及偏角，分别对局部地图进行旋转变换，得到对应的匹配度，取匹配度最高的情况，即为机器人当前的位姿，，具体算法如下：

pmax＝0

poscur＝pos0

foreach(posi:pos)

foreach(wm-σ/2:δ:wm+σ/2)

pi＝π1-(gcicj-mij)

if(pi＞pmax)

pmax＝pi

poscur＝posi

记匹配度最大为pmax，位置匹配结果记为poscur，当前默认位置pos0，默认偏角wm，当前激光雷达可能存在的位置posi，pos为当前所有可能存在的位置的集合，δ为激光雷达最小分辨角度，σ表示激光雷达角度偏差范围，gcicj表示全局地图上对应局部地图上的点存在障碍物的概率，mij表示局部地图上的点存在障碍物的概率，当前计算的匹配度为pi。

计算激光雷达处于该位置和偏角下局部地图各个点对应全局地图的点gcicj，并计算局部地图的匹配度pi，如果匹配度大于当前最高匹配度，则更新最大匹配度以及激光雷达当前位置。根据激光雷达和小车几何中心的位置关系可以得到小车几何中心的位置。

步骤6：根据机器人当前位置信息与轨迹线之间的偏差进行位姿调整，保持机器人跟随轨迹线。

如图6所示，竖直直线为预期轨迹线，矩形方框为小车实际位置，小车几何中心点与轨迹线的水平距离偏差为d,方向角偏差为θ，则对小车的倒行控制策略如下：

dr＝d/cosθ

vx＝max(vxmin,min(vxmax,kx*dr))

vw＝max(vwmin,min(vwmax,kw*θ))

vy＝max(vymin,min(vymax,(1-vw/vwmax)vyset))

其中，d为小车几何中心点与轨迹线的水平距离偏差,θ为方向角偏差，vx、为x方向、vy为y方向上小车速度，vw为小车的角速度，vxmin和vxmax分别为vx设定的最小值和最大值，vymin和vymax分别为vy设定的最小值和最大值，vwmin和vwmax分别为vw设定的最小值和最大值，kx和kw分别为x方向和角速度的比例系数，vyset为设定的y方向速度大小。

考虑到小车水平运动方向上距预期轨迹线偏差为dr，故首先计算dr的长度来确定平移速度大小，然后根据角度偏移大小确定自旋速度大小。根据自旋速度来确定纵向速度的大小，其中vyset为设定的y方向速度大小。最后将预期控制速度转化为对应的小车控制指令，即保持小车的跟踪轨迹线倒行。

本实施例的全向移动机器人通道内倒行导航方法在充分利用激光雷达采集的环境信息的情况下，结合全向移动机器人的特点，提出了基于激光雷达的全向移动机器人倒行导航方法，此方法为工程上实现全向移动机器人通道内倒行提供了一种解决方案，该方案易于实现、适应性强，能够解决通道内移动机器人倒行导航问题。

以上所述实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。