CN110031009A

CN110031009A - 一种初始定位方法、计算机可读存储介质及系统

Info

Publication number: CN110031009A
Application number: CN201910270168.7A
Authority: CN
Inventors: 肖卫国; 李�根; 章小龙; 孟杰; 刘超
Original assignee: Individual
Current assignee: Individual
Priority date: 2019-04-04
Filing date: 2019-04-04
Publication date: 2019-07-19

Abstract

本发明提供一种初始定位方法，用于定位AGV小车的初始位姿，该方法包括步骤：S1，建立AGV小车工作区域的全局地图；S2，计算AGV小车开始工作时，在全局地图中的预计初始位姿；及S3，利用预计初始位姿确定搜索范围，在小范围搜索区间内采用粒子滤波算法计算出AGV小车在全局地图中精确的初始位姿。本发明还提供一种计算机可读存储介质。本发明还提供一种初始定位系统。

Description

一种初始定位方法、计算机可读存储介质及系统

【技术领域】

本发明涉及AGV小车领域，尤其涉及一种初始定位方法、计算机可读存储介质及系统。

【背景技术】

AGV小车（Automated Guided Vehicle，自动搬运小车）通常指装备有电磁或光学等自动导引装置，能够沿规定的导引路径行驶，具有安全保护以及各种移栽功能的运输车。现有的AGV小车中也存在不需要导引装置，通过提前获取工作区间的地图从而自由设置行驶路径的AGV小车。但现有的AGV小车在获取小车位于地图的初始位姿时，通常只有一个大概的预测，并不能精准的获取小车在地图中的精确初始位姿。

【发明内容】

为克服现有问题，本发明提供一种初始定位方法、计算机可读存储介质及系统。

本发明解决技术问题的技术方案是提供一种初始定位方法，用于定位AGV小车的初始位姿，该方法包括步骤：S1，建立AGV小车工作区域的全局地图；S2，计算AGV小车开始工作时，在全局地图中的预计初始位姿；及S3，利用预计初始位姿计算出AGV小车在全局地图中精确的初始位姿。

优选地，步骤S1还包括步骤：S11，扫描工作区域，获取工作区域内所有的环境信息；及S12，利用获取的环境信息，创建工作区域的全局地图。

优选地，步骤S2还包括步骤：S21，读取全局地图，并在全局地图中插入坐标系；S22，将AGV小车启动时周边的环境信息与全局地图进行全局匹配，得到全局地图中与AGV小车启动时周边的环境信息相同和/或相近的位置；及

S23，将步骤S22中得到的位置进行局部匹配，估算出AGV小车的预计初始位姿。

优选地，步骤S23还包括步骤：S231，建立栅格地图；

S232，将全局地图中的坐标系对应插入栅格地图中；及S233，将全局地图中与AGV小车启动时周边的环境信息利用ICP匹配进行局部匹配，计算出AGV小车在栅格地图中的预计初始位姿。

优选地，步骤S22还包括步骤：S221，获取AGV小车启动时周边的环境信息；及S222，将AGV小车启动时周边的环境信息与全局地图中的所有环境信息利用似然估计算法逐一进行匹配，得到极大似然估计值。

优选地，S3还包括步骤：S31，建立粒子滤波框架；及S32，利用粒子滤波框架及预计初始位姿，计算出AGV小车启动时在全局地图中精确的位姿。

优选地，步骤S32还包括步骤：S321，以预计初始位姿为中心，按照高斯分别向周围随机播撒粒子；S322，用粒子滤波算法中采样与重采样的计算法方法，对粒子进行实时更新；及S323，对所有粒子进行加权平均计算，获取AGV小车在全局地图中确定的初始位姿。

本发明还提供一种计算机可读存储介质，其特征在于：所述计算机可读存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行初始定位方法。

本发明还提供一种初始定位系统，其特征在于：所述初始定位系统包括：AGV小车，用于在工作区域内运动；扫描模块，用于扫描AGV小车的工作区域内所有的环境信息及AGV小车启动时周边的环境信息；通讯模块，用于传输扫描模块扫描到的环境信息；服务器，用于接收扫描区域扫描到的环境信息，并根据环境信息生成全局地图及计算出AGV小车在全局地图中的初始位姿。

优选地，扫描模块为2个可以设置于AGV小车上的激光雷达。

与现有技术相比，本发明提供的一种初始定位方法具有以下优点：

1.通过先计算出AGV小车在全局地图中的预计初始位姿，在根据预计初始位姿计算出确定的初始位姿，可以保证AGV小车在启动时的定位更加精准。

2.通过全局匹配即局部匹配，可在多个极大似然值中选出较为准确的一个，便于后续的计算。

3.将全局地图转换为计算机便于识别的栅格地图格式，便于计算机运算、用户操作及直观观测结果。

4.采用粒子滤波的算法对预计初始位姿进一步计算，以得到更精确的AGV小车初始位姿。

本发明所提供的计算机可读存储介质及系统均具有如上的有益效果。

【附图说明】

图1是本发明第一实施例初始定位系统的模块示意图。

图2是本发明第二实施例初始定位方法的流程示意图。

图3是图2中步骤S1的流程示意图。

图4A是图2中步骤S2的流程示意图。

图4B是图4A中步骤S22的流程示意图。

图4C是图4A中步骤S23的流程示意图。

图5A是图2中步骤S3的流程示意图。

图5B是图5A中步骤S32的流程示意图。

附图标记：20、AGV小车；30、扫描模块；40、通讯模块；50、服务器。

【具体实施方式】

为了使本发明的目的，技术方案及优点更加清楚明白，以下结合附图及实施实例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

请参阅图1，本发明第一实施例提供一种初始定位系统10，初始定位系统10包括AGV小车20、扫描模块30、通讯模块40及服务器50。AGV小车20可在工作区域内运动，以运输物料，扫描模块30设置于AGV小车20上，用于扫描AGV小车20的工作区域，并将扫描到的信息通过通讯模块40传输至服务器50上。服务器50根据接收到的信息生成AGV小车20工作区域的全局地图，且在每次AGV小车20启动工作时，获知AGV小车在全局地图中的初始位姿。

具体的，AGV小车20为通过车轮驱动，无需在工作区域内安装导轨。扫描模块30为激光雷达，设置于AGV小车20上可跟随AGV小车20一同运动，并可扫描AGV小车周边的环境信息，并将环境信息通过通讯模块30传输至服务器50。当扫码模块30跟随AGV小车20运动以扫描过整个工作区域后，服务器50根据接收到的整个工作区域的环境信息，根据整个区域的环境信息建立整个工作区域的全局地图，且当每次AGV小车20开始启动准备工作时，可以通过AGV小车20上的扫描模块30扫描到AGV小车周边当前的环境信息，并根据当前的环境信息与全局地图比对，获取AGV小车启动时位于全局地图中的初始位姿。

进一步的，扫描模块30为两个可旋转0-270°的激光雷达，通过2个可旋转激光雷达相互配合，可更全面的扫描AGV小车20周边的环境信息。

可以理解，环境信息为激光雷达通过三角测量原理实时计算出激光雷达扫描到的内容，如周边设备、路面等与激光雷达中心的距离和角度信息。

可以理解，工作区域为AGV小车需要行驶工作的范围。

请参阅图2，本发明的第二实施例提供一种初始定位方法，该方法包括步骤：

S1，建立AGV小车工作区域的全局地图；

S2，计算AGV小车开始工作时，在全局地图中的预计初始位姿；及

S3，利用预计初始位姿计算出AGV小车在全局地图中确定初始位姿。

具体的，当AGV小车在每次工作完之后的停靠位置不同时，其下一次开始工作启动时的初始位姿也不同。为了能够获取AGV小车每次开始工作时的初始位姿，先对AGV小车的工作区域进行扫描，并建立一个全局的地图，也即整个工作区域的地图。在AGV小车每次启动开始准备工作时，会扫描AGV小车周边的环境信息，并根据AGV小车周边的环境信息与全局地图对比，计算出AGV小车启动时处于全局地图中的位置。由于只是利用AGV小车启动时周边的环境信息与全局地图对比计算出的位置并不准确，如扫描AGV小车周边的环境信息时和/或将周边的环境信息与全局地图对比时，可能出现误差。所以，通过周边的环境信息与全局地图对比得到的位置只是AGV小车开始工作时在全局地图中的预计初始位姿，还需要对预计初始位姿进行验证，从而计算出AGV小车开始工作时在全局地图中确定初始位姿。

可以理解，位姿即AGV小车的二维坐标及方向，预计初始位姿即为通过AGV小车启动时扫描到的周边的环境信息与建立好的全局地图进行对比计算得到AGV小车在全局地图中的二维坐标及方向。确定初始位姿为对预计初始位姿验证后计算得到AGV小车在全局地图中的二维坐标及方向。

可以理解，对预计初始位姿验证的方式为通过粒子滤波的算法对预计初始位姿进行验证。

请参阅图3，步骤S1还包括步骤：

S11，扫描工作区域，获取工作区域全部的环境信息；及

S12，利用获取的环境信息，创建工作区域的全局地图。

具体的，在确定AGV小车的工作区域后，先将整个工作区域的环境信息进行扫描，以得到工作区域全部的环境信息，并利用环境信息创建出整个工作区域的全局地图。

可以理解，全局地图为栅格地图生成的二维点云地图，建立全局地图后，可真实还原工作区域的轮廓原貌。

在一些应用场景中，AGV小车的工作区域为工厂车间。在AGV小车上设置激光雷达，让AGV小车将整个车间行驶以便，且在行驶的过程中激光雷达跟随AGV小车的行驶实时扫描并获取车间内的设备、路况等相对激光雷达中心的距离及角度，从而获取整个车间内环境信息。最后将获取的环境信息组合成车间内的二维点云地图，即车间的全局地图。

请参阅图4A，步骤S2还包括步骤：

S21，读取全局地图，并在全局地图中插入坐标系；

S22，将AGV小车启动时周边的环境信息与全局地图进行全局匹配，得到全局地图中与AGV小车启动时周边的环境信息相同和/或相近的位置；及

在建立好工作区域的全局地图后，在全局地图内插入坐标系，以使全局地图中的每个点都有一个唯一的坐标。在AGV小车需要工作时并启动的同时，通过扫描获取小车周边的环境信息，并将获取到的环境信息与全局地图进行全局匹配，也即将AGV小车启动时周边的环境信息与全局地图中所有的环境信息进行逐一对比，以获取全局地图中环境信息与AGV小车启动时周边的环境信息相同和/或相近的位置。由于是利用AGV小车启动时周边的环境信息逐一与全局地图中的环境信息对比，在全局地图中识别出的可能为多个位置，此时将几个位置通过进行局部匹配，即将全部地图中与AGV小车启动时周边的环境信息再次与全局地图中相似度最高的几个位置进行对比，得到相似度最高的一个位置作为AGV小车启动时在全局地图中的预计初始位姿。

在一些应用场景中，当AGV小车启动时，激光雷达开始扫描AGV小车周边的环境信息，如AGV小车相对周边设备的距离、角度等。由于全局地图是通过整个工作区域的环境信息组合而成，即全局地图中有与AGV小车启动时周边的环境信息相同的位置，进行全局匹配后可以在全局地图中找到与AGV小车启动时周边的环境信息相同和/或的位置。在选取出全局地图中这些相同和/或相似的位置后，在将这些位置的环境信息与AGV小车启动时周边的环境信息进行比对，从而选取出其中相似度最高的位置，以该位置在全局地图中插入的坐标系对应的坐标作为AGV小车启动时在全局地图中的预计初始位姿。

可以理解，相似度的高低即为判断两个环境信息对比后特征相同点的多少，如AGV小车启动时周边的环境信息为激光雷达中心距离周边设备的距离100mm、角度20°，而在全局地图中三个位置分别与周边设备的关系为A、距离90mm、角度20°；B、距离100mm、角度19°；C、距离100mm、角度20°，则认定C位置与AGV小车启动时周边的环境信息的特征相同点最多，将C位置作为AGV小车启动时在全局地图中的预计初始位姿。

可以理解，全局地图中坐标系的原点可以是全局地图中的任意位置，只要保证原点不变即可。

请参阅图4B，步骤S22还包括步骤：

S221，获取AGV小车启动时周边的环境信息；及

S222，将AGV小车启动时周边的环境信息与全局地图中的所有环境信息利用似然估计算法逐一进行匹配，得到极大似然估计值。

具体的，将AGV小车启动时周边的环境信息与全局地图进行全局匹配的具体方法为利用似然估计算法进行计算，即将AGV小车启动时周边的环境信息及全局地图中所有的环境信息放入似然函数中进行计算，得出的结果为极大似然估计值，也即全局地图中与AGV小车启动时周边的环境信息相同和/或相近的位置。

似然函数（linkehood function）：联合概率密度函数p(D|θ)称为相对于{x₁，x₂，…，x_N}的θ的似然函数：

如果是参数空间中能使似然函数l(θ)最大的θ值，则应该是“最可能”的参数值，那么就是θ的极大似然估计量。它是样本集的函数，记作：

请参阅图4C，步骤S23还包括步骤：

S231，建立栅格地图；

S232，将全局地图中的坐标系对应插入栅格地图中；及

S233，将全局地图中与AGV小车启动时周边的环境信息利用ICP匹配进行局部匹配，计算出AGV小车在栅格地图中的预计初始位姿。

具体的，栅格地图也称光栅图形，建立光栅地图即为将全局地图转换为计算机可识别的灰度图，并在使栅格地图与二维点云地图对应，且插入坐标系后的原点处于地图中的位置对应。最后，利用ICP匹配算法计算出AGV小车在栅格地图中的预计初始位姿，通过栅格地图方便用户在计算机上进行计算且直观的观测到计算的结果。

ICP匹配算法（Iterative Closest Point）公式：

X为地图点云，即地图中的点；P为当前帧

X＝{x₁，...，x_n}

P＝{p₁，...，p_n}

最小均方差公式：

R表示位姿旋转向量，t表示位姿平移。

请参阅图5A，步骤S3还包括步骤：

S31，建立粒子滤波框架；及

S32，利用粒子滤波框架及预计初始位姿，计算出AGV小车启动时在全局地图中的确定初始位姿。

具体的，由于预计初始位姿是用过似然估计算法及ICP匹配算法分别进行全局匹配即局部匹配得到的结果，在实际的操作过程中，对比的结果由于设备本身识别的问题或比对过程中出现比对错误的问题，无法精准的得到AGV小车在启动时处于全局地图中确切的初始位姿。所以，在得到预计初始位姿后，再对预计初始位姿进行进一步的验证，以确保获取AGV小车在启动时在全局地图中确定的初始位姿。

可以理解，粒子滤波（Particle Filter）是指通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数。粒子滤波框架即为粒子滤波的计算函数。

请参阅图5B，步骤S32还包括步骤：

S321，以预计初始位姿为中心点，按照高斯分布像周围随机播撒粒子；

S322，用粒子滤波算法中采样与重采样的计算法方法，对粒子进行实时更新；及

S323，对所有粒子进行加权平均计算，获取AGV小车在全局地图中确定的初始位姿。

具体的，在获取到预计初始位姿后，在粒子滤波框架中以预计初始位姿为中心点，并按照高斯分布的方式向中心点四周随机播撒粒子。可以理解，播撒的粒子为粒子滤波框架中创建的参照样本，即并非真实存在，只是为了方便计算所创建的一种参考样本。在播撒粒子后，每个粒子距离中心点都有一个距离，以该距离的大小为依据对每个粒子赋予权值，即与靠近中心点的权值越大，反之则越小。在通过粒子滤波中的采用与重采样后对所有粒子的权值进行更新，并在AGV小车运动一段距离后，对所有粒子的权值进行加权平均计算，即可得到的即为AGV小车启动时的确定初始位姿。

粒子滤波框架包括的算法为：

预测：由上一时刻的概率密度p(x_k-1|y_1：k-1)p(x_k-1|y_1：k-1)得到p(x_k|y_1：k-1)，即由k-1时刻的测量数据，预测下一状态x(k)出现的概率。

给出推导公式：

p(x_k|y_1：k-1)＝∫p(x_k，x_k-1|y_1：k-1)dx_k-1

＝∫p(x_k|x_k-1，y_1：k-1)p(x_k-1|y_1：k-1)dx_k-1

＝∫p(x_k|x_k-1)p(x_k-1|y_1：k-1)dx_k-1

更新：由p(x_k|y_1：k-1)得到后验概率p(x_k|y_1：k)先验概率只是预测值，现在添加了k时刻的测量值，对预测值进行了修正，即为滤波。

其中归一化常数：

p(y_k|y_1：k-1)＝∫p(y_k|x_k)p(x_k|y_1：k-1)dx_k

粒子滤波重采样：

本发明的第三实施例还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行如上所述初始定位方法。

根据本公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分从信息传输模块上被下载和安装，和/或从可拆卸介质被安装。在该计算机程序被中央处理单元(CPU)执行时，执行本申请的方法中限定的上述功能。需要说明的是，本申请所述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本申请中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本申请中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。

可以以一种或多种程序设计语言或其组合来编写用于执行本申请的操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的信息传输模块包括局域网(LAN)或广域网(WAN)连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。

与现有技术相比，本发明所述提供的一种初始定位方法具有以下优点：

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

Claims

1.一种初始定位方法，用于定位AGV小车的初始位姿，其特征在于：该方法包括步骤：

S1，建立AGV小车工作区域的全局地图；

S3，利用预计初始位姿计算出AGV小车在全局地图中精确的初始位姿。

2.如权利要求1所述的一种初始定位方法，其特征在于：步骤S1还包括步骤：

S11，扫描工作区域，获取工作区域内所有的环境信息；及

S12，利用获取的环境信息，创建工作区域的全局地图。

3.如权利要求1所述的一种初始定位方法，其特征在于：步骤S2还包括步骤：

S21，读取全局地图，并在全局地图中插入坐标系；

4.如权利要求3所述的一种初始定位方法，其特征在于：步骤S23还包括步骤：

S231，建立栅格地图；

S232，将全局地图中的坐标系对应插入栅格地图中；及

5.如权利要求3所述的一种初始定位方法，其特征在于：步骤S22还包括步骤：

S221，获取AGV小车启动时周边的环境信息；及

6.如权利要求1所述的一种初始定位方法，其特征在于：

步骤S3还包括步骤：

S31，建立粒子滤波框架；及

S32，利用粒子滤波框架及预计初始位姿，计算出AGV小车启动时在全局地图中精确的位姿。

7.如权利要求6所述的一种初始定位方法，其特征在于：

步骤S32还包括步骤：

S321，以预计初始位姿为中心，按照高斯分别向周围随机播撒粒子；

8.一种计算机可读存储介质，其特征在于：所述计算机可读存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行所述权利要求1-8中任一项中所述初始定位方法。

9.一种初始定位系统，其特征在于：所述初始定位系统包括：

AGV小车，用于在工作区域内运动；

扫描模块，用于扫描AGV小车的工作区域内所有的环境信息及AGV小车启动时周边的环境信息；

通讯模块，用于传输扫描模块扫描到的环境信息；

服务器，用于接收扫描区域扫描到的环境信息，并根据环境信息生成全局地图及计算出AGV小车在全局地图中的初始位姿。

10.如权利要求9所述的一种初始定位系统，其特征在于：所述扫描模块为2个可以设置于AGV小车上的激光雷达。