CN110045733B - 一种实时定位方法及其系统、计算机可读介质 - Google Patents

一种实时定位方法及其系统、计算机可读介质 Download PDF

Info

Publication number
CN110045733B
CN110045733B CN201910269965.3A CN201910269965A CN110045733B CN 110045733 B CN110045733 B CN 110045733B CN 201910269965 A CN201910269965 A CN 201910269965A CN 110045733 B CN110045733 B CN 110045733B
Authority
CN
China
Prior art keywords
real
pose
time
automatic carrying
carrying trolley
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910269965.3A
Other languages
English (en)
Other versions
CN110045733A (zh
Inventor
肖卫国
李�根
李强
孟杰
刘超
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Individual
Original Assignee
Individual
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Individual filed Critical Individual
Priority to CN201910269965.3A priority Critical patent/CN110045733B/zh
Publication of CN110045733A publication Critical patent/CN110045733A/zh
Application granted granted Critical
Publication of CN110045733B publication Critical patent/CN110045733B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar

Abstract

本发明涉及一种实时定位方法，用于获取自动搬运小车行驶时的实时位姿，该方法包括以下步骤，S1：建立区域地图，并基于区域地图获取各个工位位置信息；S2：获得自动搬运小车的初始位姿，在小车朝目标工位行驶的过程中，当自动搬运小车相对目标工位的距离大于设定的阈值时，使用粗定位方法，获取自动搬运小车的粗定位实时位姿；及S3：当自动搬运小车行驶至相对目标区域的距离小于或等于设定的阈值时，将粗定位方法切换为精定位方法，以获取自动搬运小车位于区域地图中的精定位实时位姿。本发明还提供一种计算机可读介质。本发明还提供一种实时定位系统。

Description

一种实时定位方法及其系统、计算机可读介质
技术领域
本发明涉及物流自动化技术领域，涉及一种实时定位方法及其系统、计算机可读介质。
背景技术
现有的自动搬运小车(Automated Guided Vehicl简称AGV)在进行搬运作业之前，都需要计算自动搬运小车的实时位姿，进而才能够顺利进行搬运工作，获取自动搬运小车实时位姿的方法，都是在每个轮子上安装两个编码器，一个编码器建立自动搬运小车的直行方程，另一编码器建立自动搬运小车的转弯方程，通过求解运动学方程得出自动搬运小车的实时位姿。这种方法得出自动搬运小车在任何时刻的实时位姿精准度都是一样，无法根据需要，获得自动搬运小车的实时位姿。
发明内容
为克服现有技术存在的问题，本发明提供一种实时定位方法及其系统、计算机可读介质。
本发明解决技术问题的方案是提供一种实时定位方法，用于获取自动搬运小车行驶时的实时位姿，其特征在于：该方法包括以下步骤，S1：建立区域地图，并基于区域地图获取各个工位位置信息；S2：获得自动搬运小车的初始位姿，在小车朝目标工位行驶的过程中，当自动搬运小车相对目标工位的距离大于设定的阈值时，使用粗定位方法，获取自动搬运小车的粗定位实时位姿；及S3：当自动搬运小车行驶至相对目标区域的距离小于或等于设定的阈值时，将粗定位方法切换为精定位方法，通过对粗定位实时位姿进行ICP匹配并根据ICP匹配结果获取自动搬运小车位于区域地图中的精定位实时位姿；
S311：将自动搬运小车的任一时刻对应的粗定位实时位姿作为当前帧位姿；
S312：获取自动搬运小车在目标工位时的位姿作为参考帧位姿；及
S313：采用ICP匹配算法计算参考帧位姿和当前帧位姿之间的位姿增量，以对应进行ICP匹配。
优选地，步骤S1包括步骤，S11：扫描工作区域，获取工作区域内所有的环境信息；S12：利用获取的环境信息，创建工作区域的区域地图，及S13：基于区域地图获取各个工位位置信息。
优选地，步骤S2包括步骤，S21：采集激光雷达的数据构建航迹推算预测方程，并计算获得自动搬运小车带噪声干扰的实时位姿；所述航迹推算预测方程具体为：
Figure GDA0003742885680000011
其中，
Figure GDA0003742885680000012
表示状态变量xt的置信度，表示预测；η表示贝叶斯准则中的归一化变量；xt表示t时刻的状态变量；ut表示t时刻的控制；
S22：采集激光雷达的数据，结合步骤S21预测带噪声干扰的实时位姿，构建观测修正方程；所述观测修正方程具体为：
Figure GDA0003742885680000021
其中，bel(xt)表示状态变量xt的置信度，表示预测；xt表示t时刻的状态变量，zt表示t时刻的观测；及S23：通过给定的初始位姿，基于蒙特卡洛采样原理，使用粒子滤波方法求解观测修正方程，估算出自动搬运小车的粗定位实时位姿。
优选地，在上述步骤S21中，利用激光里程计采集激光雷达在预设时刻对应的前一帧实时位姿和后一帧实时位姿进行ICP匹配，模拟航迹推算，计算自动搬运小车当前的里程信息，并基于所述里程信息构建航迹推算预测方程。
优选地，在上述步骤S23中，通过粒子滤波以分析比较带噪声干扰的实时位姿的概率密度，以降低噪声干扰对粗定位实时位姿的影响。
优选地，步骤S32包括步骤，S321：基于对应的位姿增量获得对应的变换矩阵，将参考帧位姿按照所述变换矩阵转换为当前帧位姿；及S322：计算出自动搬运小车的精定位实时位姿。
本发明还提供一种计算机可读介质，所述计算机可读介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行上述的实时定位方法。
本发明还提供一种实时定位系统，所述实时定位系统包括自动搬运小车，用于在不同工位之间搬运工件；扫描模块，用于扫描区域环境信息；通讯模块，用于将区域环境信息传送至服务器；服务器，用于根据区域环境信息建立区域地图，并获取自动搬运小车在区域地图中的实时位姿，所述实时位姿由上述的实时定位方法获得。
与现有技术相比，本发明的实时定位方法具有以下优点：
1、通过建立区域地图获取各个工位的位置信息，并且通过对自动搬运小车进行实时定位，以使自动搬运小车按顺序在不同工位之间搬运工件，当自动搬运小车相对目标工位的距离大于设定的阈值时，使用粗定位方法获取自动搬运小车的粗定位实时位姿，以使自动搬运小车朝向目标工位的方向行驶；当自动搬运小车行驶至相对目标区域的距离小于或等于设定的阈值时，切换为精定位方法获取自动搬运小车的精定位实时位姿，以使自动搬运小车准确到达目标工位，继而将工件准确的放入目标工位，以实现根据需要获得自动搬运小车的实时位姿。
2、通过航迹推算预测方程计算获得自动搬运小车带噪声干扰的实时位姿，并建立观测修正方程，然后通过粒子滤波分析出带噪声干扰的实时位姿中概率密度更高的粗定位实时位姿，进而使自动搬运小车可以朝向目标工位的方向行驶。
3、通过对参考帧位姿和当前帧位姿进行ICP匹配，继而将参考帧位姿按照变换矩阵转换为当前帧位姿，计算出自动搬运小车在阈值范围内的精定位实时位姿。
【附图说明】
图1是本发明第一实施例实时定位系统的模块示意图。
图2是本发明第二实施例实时定位方法的步骤流程图。
图3是本发明第二实施例实时定位方法中步骤S1的步骤流程图。
图4是本发明第二实施例实时定位方法中步骤S2的步骤流程图。
图5是本发明第二实施例实时定位方法中步骤S3的步骤流程图。
图6是本发明第二实施例实时定位方法中步骤S31的步骤流程图。
图7是本发明第二实施例实时定位方法中步骤S32的步骤流程图。
附图标识说明：
1、实时定位系统；11、自动搬运小车；12、扫描模块；13、通讯模块；14、服务器。
【具体实施方式】
为了使本发明的目的，技术方案及优点更加清楚明白，以下结合附图及实施实例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
请参阅图1，本发明第一实施例提供一种实时定位系统1，其包括自动搬运小车11、扫描模块12、通讯模块13及服务器14。自动搬运小车11用于在不同工位之间搬运工件；扫描模块12设置于自动搬运小车11上，可跟随搬运小车12一同运动，并可扫描自动搬运小车11周边的区域环境信息。通讯模块13用于传递信息，扫描模块12在扫描到自动搬运小车11周边的区域环境信息后，通过通讯模块13将区域环境信息传送至服务器14。服务器14在接收到区域环境信息后，根据区域环境信息建立区域地图，并获取自动搬运小车11在区域地图中的实时位姿。
具体地，将扫描模块12安装于自动搬运小车11上，在建立区域地图之前，自动搬运小车11在工作区域内行驶一遍，行驶过程中，扫描模块12利用激光雷达扫描工作区域环境信息，并通过通讯模块13，将区域环境信息传送至服务器14，服务器14在接收到区域环境信息后，根据扫描到的区域环境信息建立区域地图，并获取自动搬运小车11在区域地图中的实时位姿。
当自动搬运小车11在区域地图中进行搬运作业，并且与目标工位之间的距离大于设定的阈值时，自动搬运小车11进入粗定位模式，服务器14获取自动搬运小车11的粗定位实时位姿；当自动搬运小车11与目标工位之间的距离小于或等于设定的阈值，自动搬运小车11切换为精定位模式，服务器14获取自动搬运小车11的精定位实时位姿。
进一步地，阈值为20-80cm，优选地，阈值设置为50cm，即当自动搬运小车11与目标工位之间的距离大于50cm时，自动搬运小车11进入粗定位模式，服务器14获取自动搬运小车11的粗定位实时位姿，继而使自动搬运小车11朝向目标工位的方向行驶；当自动搬运小车11与目标工位之间的距离小于或等于50cm时，自动搬运小车11切换为精定位模式，服务器14获取自动搬运小车11的精定位实时位姿，继而使自动搬运小车11准确地达到目标工位。
更进一步地，激光雷达数量可为多个，扫描角度可为0°-360°，在一些具体的实施例中，所述激光雷达的可为2个，其扫描的角度为270度，设置于自动搬运小车11的斜对角位置，以扫描区域环境信息。
在本实施例中，工作区域为车间，区域地图即为车间地图，可以理解，工作区域还可以为办公室、写字楼等区域；各个工位均设有信号发射器，扫描模块12扫描到各个工位发射的信号，进而得出工位位置信息；区域地图还可以通过利用红外扫描、激光扫描等仪器扫描。
可以理解，自动搬运小车11在不同工位之间搬运工件的顺序可以为预先设定好的，以使工件可以按照加工顺序进行加工，或者工位的位置沿自动搬运小车11的运动路径依次设置，以使自动搬运小车11依次在各个工位之间搬运工件，目标工位为自动搬运小车11上的工件按照加工顺序进行下一步加工时的工位，即目标工位为相对的，工件在不同工位上的加工，目标工位发生变化。
区域环境信息为激光雷达通过三角测量原理实时计算出激光雷达扫描到的内容，如周边设备、路面等与激光雷达中心的距离和角度信息。
自动搬运小车11的实时位姿为自动搬运小车11在不同时刻的位置和姿态。在本发明中，所述姿态可以理解为自动搬运小车11的方向，根据自动搬运小车11的实时位姿，确定自动搬运小车11的行驶路径以搬运工件。工件可以为电子产品、电子产品配件、包装盒等，只要自动搬运小车11能够搬运即可，本发明中的工件为手机壳。
请参阅图2，本发明第二实施例提供一种实时定位方法，用于获取自动搬运小车行驶时的实时位姿，该方法包括以下步骤，
S1：建立区域地图，并基于区域地图获取各个工位位置信息；
S2：获得自动搬运小车的初始位姿，在小车朝目标工位行驶的过程中，当自动搬运小车相对目标工位的距离大于设定的阈值时，使用粗定位方法，获取自动搬运小车的粗定位实时位姿；及
S3：当自动搬运小车行驶至相对目标区域的距离小于或等于设定的阈值时，将粗定位方法切换为精定位方法，以获取自动搬运小车位于区域地图中的精定位实时位姿。
具体地，首先利用激光雷达扫描工作区域的地形以建立区域地图，并基于区域地图获取各个工位位置信息，使自动搬运小车在区域地图中行驶，并在不同工位之间搬运工件，完成工件的流水线加工操作。当自动搬运小车相对目标工位的距离大于设定的阈值时，使用粗定位方法，获取自动搬运小车位于区域地图中的粗定位实时位姿；当自动搬运小车行驶至相对目标区域的距离小于或等于设定的阈值时，将粗定位方法切换为精定位方法，以获取自动搬运小车位于区域地图中的精定位实时位姿。
请参阅图3，步骤S1进一步包括步骤，
S11：扫描工作区域，获取工作区域内所有的环境信息；
S12：利用获取的环境信息，创建工作区域的区域地图，及
S13：基于区域地图获取各个工位位置信息。
具体地，创建工作区域的区域地图之前，先利用激光雷达扫描工作区域，以获取工作区域内所有的环境信息，然后根据获取的环境信息，创建工作区域的区域地图，继而基于区域地图获取各个工位位置信息，以使自动搬运小车可以在创建的区域地图中行驶，并在不同的工位之间搬运工件，完成工件的流水线加工操作。
请参阅图4，步骤S2包括步骤，
S21：采集激光雷达的数据构建航迹推算预测方程，并计算获得自动搬运小车带噪声干扰的实时位姿；
航迹推算的运动模型为：
Figure GDA0003742885680000051
be l-(xt)–状态变量xt的置信度，表示预测
η-贝叶斯准则中的归一化变量
xt-t时刻的状态变量
ut-t时刻的控制
S22：采集激光雷达的数据，结合步骤S21预测带噪声干扰的实时位姿，构建观测修正方程；及
观测模型为：
Figure GDA0003742885680000052
be l-(xt)–状态变量xt的置信度，表示预测
xt-t时刻的状态变量
zt-t时刻的观测
S23：通过给定的初始位姿，基于蒙特卡洛采样原理，使用粒子滤波方法求解观测修正方程，估算出自动搬运小车的粗定位实时位姿。
首先，利用激光里程计采集激光雷达在预设时刻对应的前一帧实时位姿和后一帧实时位姿进行ICP匹配，模拟航迹推算，计算自动运输小车当前的里程信息，并基于所述里程信息构建航迹推算预测方程，继而计算获得自动搬运小车的带噪声干扰的实时位姿。
需要说明的是，此处所述的预设时刻可为固定的某一时刻，如可为所述自动搬运小车进入阈值范围的临界时刻，或者也可为任意选取的自动运输小车开始移动后的第10秒、第20秒及第30秒。在预设的这一时刻的前一帧对应的实时位姿和后一帧对应的实时位姿中，前一帧与后一帧可以是这一预设时刻的前后1秒、0.5秒或者3秒，具体不做限定。
第二，采集激光雷达的数据，结合航迹推算预测方程预测的带噪声干扰的实时位姿，构建观测修正方程；
最后通过粒子滤波以分析比较带噪声干扰的实时位姿的概率密度，以去除影响粗定位实时位姿的噪声，以得到粗定位实时位姿。
其中，基于蒙特卡洛粒子滤波递推公式如下：
bel(xt)＝η∫p(zt|xt)p(xt|xt-1，ut)bel(xt-1)dxt-1
bel-(xt)–状态变量xt的置信度，表示预测
η-贝叶斯准则中的归一化变量
xt-t时刻的状态变量
ut-t时刻的控制
给定t＝0时刻的初始置信度bel(x0)，在贝叶斯框架下进行预测和更新。
具体地，在使用粒子滤波进行分析比较时，由上一时刻的概率密度p(xk-1|y1：k-1)得到p(xk|y1：k-1)，即由k-1时刻的测量数据，预测下一状态x(k)出现的概率。
给出推导公式：
p(xk|y1：k-1)＝∫p(xk，xk-1|y1：k-1)dxk-1
＝∫p(xk|xk-1，y1：k-1)p(xk-1|y1：k-1)dxk-1
＝∫p(xk|xk-1)p(xk-1|y1：k-1)dxk-1
更新：
由p(xk|y1：k-1)得到后验概率p(xk|y1：k)先验概率只是预测值，现在添加了k时刻的测量值，对预测值进行了修正，即为滤波。
Figure GDA0003742885680000061
其中归一化常数：
p(yk|y1：k-1)＝∫p(yk|xk)p(xk|y1：k-1)dxk
粒子滤波重采样：
Figure GDA0003742885680000062
For i＝1：N
(1)采样：
Figure GDA0003742885680000063
(2)根据
Figure GDA0003742885680000064
递推计算各个粒子的权重。
进而分析出带噪声干扰的实时位姿概率密度得到粗定位实时位姿，同时，计算出自动搬运小车当前的位姿与目标工位之间的距离，并判断自动搬运小车当前的位姿与目标工位之间的距离是否在阈值范围内，若不在阈值范围内，则继续使用粗定位方法获取自动搬运小车的粗定位实时位姿；反之，则切换为精定位方法获取自动搬运小车的精定位实时位姿。
可以理解，粒子滤波为过滤粗定位实时位姿中的噪声干扰，以得出准确度更高的粗定位实时位姿；利用激光里程计采集激光雷达在预设时刻对应的前一帧和后一帧进行ICP匹配，模拟航迹推算方恒，计算自动运输小车当前的里程信息，并基于所述里程信息构建航迹推算预测方程，即可估算出自动搬运小车的粗定位实时位姿。
请参阅图5，步骤S3包括步骤，
S31：对粗定位实时位姿进行ICP匹配；及
S32：根据ICP匹配的结果求出自动搬运小车的精定位实时位姿。
具体地，在切换到精定位方法后，先获取参考帧X和当前帧P，
ICP算法位姿估计公式：X为参考帧；P为当前帧
X＝{x1，...，xn}
P＝{p1，...，pn}
最小均方差公式：
Figure GDA0003742885680000071
式中，R表示位姿旋转向量，t表示位姿平移
然后通过对最小均方差的计算，即可求解出自动搬运小车的精定位实时位姿。
请参阅图6，步骤S31包括步骤，
S311：获取自动搬运小车在粗定位时的实时位姿为当前帧位姿；
S312：获取自动搬运小车在目标工位时的位姿作为参考帧位姿；及
S313：采用ICP匹配算法计算参考帧位姿和当前帧位姿之间的位姿增量，以对应进行ICP匹配。
具体地，先利用激光雷达获取自动搬运小车在粗定位实时位姿为当前帧位姿；然后获取自动搬运小车在目标工位时的位姿作为参考帧位姿；最后采用ICP匹配算法计算参考帧位姿和当前帧位姿之间的位姿增量，以对应进行ICP匹配，以求出自动搬运小车的精定位实时位姿。
请参阅图7，步骤S32包括步骤，
S321：基于对应的位姿增量获得对应的变换矩阵，其中变换矩阵求解方程为：
位姿变换矩阵估计公式：X为参考帧；P为当前帧
X＝{x1，...，xn}
P＝{p1，...，pn}
最小均方差公式：
Figure GDA0003742885680000081
式中，R表示位姿旋转向量，t表示位姿平移
将参考帧位姿按照变换矩阵转换为当前帧位姿；及
S322：计算出自动搬运小车的精定位实时位姿。
首先，基于对应的位姿增量获得对应变换矩阵，将参考帧位姿按照变换矩阵转换为当前帧位姿；然后根据变换矩阵计算出自动搬运小车的精定位实时位姿。
可以理解，参考帧为自动搬运小车停靠于目标工位时采集并存储的，在I CP匹配时，可直接调取参考帧位姿与激光雷达扫描的当前帧位姿进行匹配。
第一实施例中的内容同样适用于本实施例。
本发明第三实施例提供一种计算机可读介质，计算机可读介质中存储有计算机程序，其中，计算机程序被设置为运行时执行上述的实时定位方法。
根据本公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分从网络上被下载和安装，和/或从可拆卸介质被安装。在该计算机程序被中央处理单元(CPU)执行时，执行本申请的方法中限定的上述功能。需要说明的是，本申请所述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合，但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本申请中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本申请中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。
可以以一种或多种程序设计语言或其组合来编写用于执行本申请的操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
与现有技术相比，本发明的实时定位方法具有以下优点：
1、通过建立区域地图获取各个工位的位置信息，并且通过对自动搬运小车进行实时定位，以使自动搬运小车按顺序在不同工位之间搬运工件，当自动搬运小车相对目标工位的距离大于设定的阈值时，使用粗定位方法获取自动搬运小车的粗定位实时位姿，以使自动搬运小车朝向目标工位的方向行驶；当自动搬运小车行驶至相对目标区域的距离小于或等于设定的阈值时，切换为精定位方法获取自动搬运小车的精定位实时位姿，以使自动搬运小车准确到达目标工位，继而将工件准确的放入目标工位，以实现根据需要获得自动搬运小车的实时位姿。
2、通过航迹推算预测方程计算获得自动搬运小车带噪声干扰的实时位姿，并建立观测修正方程，然后通过粒子滤波分析出带噪声干扰的实时位姿中概率密度更高的粗定位实时位姿，进而使自动搬运小车可以朝向目标工位的方向行驶。
3、通过对参考帧位姿和当前帧位姿进行ICP匹配，继而将参考帧位姿按照变换矩阵转换为当前帧位姿，计算出自动搬运小车在阈值范围内的精定位实时位姿。以上所述仅为本发明较佳实施例而已，并不用以限制本发明，凡在本发明原则之内所作的任何修改，等同替换和改进等均应包含本发明的保护范围之内。

Claims (8)

1.一种实时定位方法，用于获取自动搬运小车行驶时的实时位姿，其特征在于：该方法包括以下步骤，
S1：建立区域地图，并基于区域地图获取各个工位位置信息；
S2：获得自动搬运小车的初始位姿，在小车朝目标工位行驶的过程中，当自动搬运小车相对目标工位的距离大于设定的阈值时，使用粗定位方法，获取自动搬运小车的粗定位实时位姿；及
S3：当自动搬运小车行驶至相对目标区域的距离小于或等于设定的阈值时，将粗定位方法切换为精定位方法，通过对粗定位实时位姿进行ICP匹配并根据ICP匹配结果获取自动搬运小车位于区域地图中的精定位实时位姿；
S311：将自动搬运小车的任一时刻对应的粗定位实时位姿作为当前帧位姿；
S312：获取自动搬运小车在目标工位时的位姿作为参考帧位姿；及
S313：采用ICP匹配算法计算参考帧位姿和当前帧位姿之间的位姿增量，以对应进行ICP匹配。
2.如权利要求1所述的实时定位方法，其特征在于：步骤S1包括步骤，
S11：扫描工作区域，获取工作区域内所有的环境信息；
S12：利用获取的环境信息，创建工作区域的区域地图，及
S13：基于区域地图获取各个工位位置信息。
3.如权利要求1所述的实时定位方法，其特征在于：步骤S2包括步骤，
S21：采集激光雷达的数据构建航迹推算预测方程，并计算获得自动搬运小车带噪声干扰的实时位姿；所述航迹推算预测方程具体为：
Figure FDA0003716654610000023
其中，
Figure FDA0003716654610000021
表示状态变量xt的置信度，表示预测；η表示贝叶斯准则中的归一化变量；xt表示t时刻的状态变量；ut表示t时刻的控制；
S22：采集激光雷达的数据，结合步骤S21预测带噪声干扰的实时位姿，构建观测修正方程；所述观测修正方程具体为：
Figure FDA0003716654610000022
其中，bel(xt)表示状态变量xt的置信度，表示预测；xt表示t时刻的状态变量，zt表示t时刻的观测；及
S23：通过给定的初始位姿，基于蒙特卡洛采样原理，使用粒子滤波方法求解观测修正方程，估算出自动搬运小车的粗定位实时位姿。
4.如权利要求3所述的实时定位方法，其特征在于：在上述步骤S21中，利用激光里程计采集激光雷达在预设时刻对应的前一帧实时位姿和后一帧实时位姿进行ICP匹配，模拟航迹推算，计算自动搬运小车当前的里程信息，并基于所述里程信息构建航迹推算预测方程。
5.如权利要求3所述的实时定位方法，其特征在于：在上述步骤S23中，通过粒子滤波以分析比较带噪声干扰的实时位姿的概率密度，以降低噪声干扰对粗定位实时位姿的影响。
6.如权利要求1所述的实时定位方法，其特征在于：步骤S3包括步骤，
S321：基于对应的位姿增量获得对应的变换矩阵，将参考帧位姿按照所述变换矩阵转换为当前帧位姿；及
S322：计算出自动搬运小车的精定位实时位姿。
7.一种计算机可读介质，其特征在于：所述计算机可读介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行权利要求1-6中任一项中所述的实时定位方法。
8.一种实时定位系统，其特征在于：所述实时定位系统包括自动搬运小车，用于在不同工位之间搬运工件；扫描模块，用于扫描区域环境信息；通讯模块，用于将区域环境信息传送至服务器；服务器，用于根据区域环境信息建立区域地图，并获取自动搬运小车在区域地图中的实时位姿，所述实时位姿由权利要求1-6任一项所述实时定位方法获得。
CN201910269965.3A 2019-04-04 2019-04-04 一种实时定位方法及其系统、计算机可读介质 Active CN110045733B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910269965.3A CN110045733B (zh) 2019-04-04 2019-04-04 一种实时定位方法及其系统、计算机可读介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910269965.3A CN110045733B (zh) 2019-04-04 2019-04-04 一种实时定位方法及其系统、计算机可读介质

Publications (2)

Publication Number Publication Date
CN110045733A CN110045733A (zh) 2019-07-23
CN110045733B true CN110045733B (zh) 2022-11-01

Family

ID=67276208

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910269965.3A Active CN110045733B (zh) 2019-04-04 2019-04-04 一种实时定位方法及其系统、计算机可读介质

Country Status (1)

Country Link
CN (1) CN110045733B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110736427B (zh) * 2019-10-25 2021-05-18 中国核动力研究设计院 反应堆探测器组件拆除装置机器视觉定位系统及定位方法
CN111220148A (zh) * 2020-01-21 2020-06-02 珊口（深圳）智能科技有限公司 移动机器人的定位方法、系统、装置及移动机器人
CN111581312A (zh) * 2020-04-24 2020-08-25 贵州力创科技发展有限公司 一种基于多种位置信息的精度定位方法和装置
CN111966109B (zh) * 2020-09-07 2021-08-17 中国南方电网有限责任公司超高压输电公司天生桥局 基于柔性直流换流站阀厅的巡检机器人定位方法及装置
CN114265395A (zh) * 2020-09-15 2022-04-01 上海海拉电子有限公司 一种智能终端、车辆躲避智能终端的方法及系统、用于车辆的定位模块及车辆
CN113485327B (zh) * 2021-06-30 2024-02-02 三一机器人科技有限公司 末端工位定位方法、装置及电子设备
CN113465606A (zh) * 2021-06-30 2021-10-01 三一机器人科技有限公司 末端工位定位方法、装置及电子设备

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1685367A (zh) * 2002-10-02 2005-10-19 西门子共同研究公司 基于定向边缘的快速二维目标定位
CN102773862A (zh) * 2012-07-31 2012-11-14 山东大学 用于室内移动机器人的快速精确定位系统及其工作方法
CN104764457A (zh) * 2015-04-21 2015-07-08 北京理工大学 一种用于无人车的城市环境构图方法
CN105911518A (zh) * 2016-03-31 2016-08-31 山东大学 机器人定位方法
CN105974922A (zh) * 2016-06-29 2016-09-28 上海诺力智能科技有限公司 一种巷道自动导引车、导引系统及其操作方法
CN105976353A (zh) * 2016-04-14 2016-09-28 南京理工大学 基于模型和点云全局匹配的空间非合作目标位姿估计方法
CN106042643A (zh) * 2016-06-06 2016-10-26 上海银帆信息科技有限公司 一种基于单点定位技术的移动机器人打印系统
CN106525053A (zh) * 2016-12-28 2017-03-22 清研华宇智能机器人（天津）有限责任公司 一种基于多传感器融合的移动机器人室内定位方法
CN106780608A (zh) * 2016-11-23 2017-05-31 北京地平线机器人技术研发有限公司 位姿信息估计方法、装置和可移动设备
CN107246873A (zh) * 2017-07-03 2017-10-13 哈尔滨工程大学 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法
CN107478214A (zh) * 2017-07-24 2017-12-15 杨华军 一种基于多传感器融合的室内定位方法及系统
CN107608350A (zh) * 2017-09-14 2018-01-19 南京理工大学 一种基于msp430的仓储物流机器人控制系统
CN108917759A (zh) * 2018-04-19 2018-11-30 电子科技大学 基于多层次地图匹配的移动机器人位姿纠正算法
CN109186625A (zh) * 2018-10-24 2019-01-11 北京奥特贝睿科技有限公司 智能车辆利用混合采样滤波进行精确定位的方法及系统

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101772977B1 (ko) * 2010-10-07 2017-08-31 삼성전자주식회사 이동 로봇 및 그 지도 작성 방법
CN104019821B (zh) * 2013-02-28 2018-02-09 腾讯科技（深圳）有限公司 一种电子地图的匹配方法和装置
DE102013211414A1 (de) * 2013-06-18 2014-12-18 Kuka Laboratories Gmbh Fahrerloses Transportfahrzeug und Verfahren zum Betreiben einesfahrerlosen Transportfahrzeugs
CN104484648B (zh) * 2014-11-27 2017-07-25 浙江工业大学 基于轮廓识别的机器人可变视角障碍物检测方法
CN104914865B (zh) * 2015-05-29 2017-10-24 国网山东省电力公司电力科学研究院 变电站巡检机器人定位导航系统及方法
CN105258702B (zh) * 2015-10-06 2019-05-07 深圳力子机器人有限公司 一种基于slam导航移动机器人的全局定位方法
US10422639B2 (en) * 2016-12-30 2019-09-24 DeepMap Inc. Enrichment of point cloud data for high-definition maps for autonomous vehicles
CN106921155B (zh) * 2017-03-10 2019-04-05 上海交通大学 直流双极供电系统电压不平衡的控制方法
US10365650B2 (en) * 2017-05-25 2019-07-30 GM Global Technology Operations LLC Methods and systems for moving object velocity determination
CN107918938A (zh) * 2017-11-16 2018-04-17 凌云光技术集团有限责任公司 一种点集与点集的匹配方法及装置
CN109145969B (zh) * 2018-08-03 2020-07-28 百度在线网络技术（北京）有限公司 三维物体点云数据的处理方法、装置、设备及介质

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1685367A (zh) * 2002-10-02 2005-10-19 西门子共同研究公司 基于定向边缘的快速二维目标定位
CN102773862A (zh) * 2012-07-31 2012-11-14 山东大学 用于室内移动机器人的快速精确定位系统及其工作方法
CN104764457A (zh) * 2015-04-21 2015-07-08 北京理工大学 一种用于无人车的城市环境构图方法
CN105911518A (zh) * 2016-03-31 2016-08-31 山东大学 机器人定位方法
CN105976353A (zh) * 2016-04-14 2016-09-28 南京理工大学 基于模型和点云全局匹配的空间非合作目标位姿估计方法
CN106042643A (zh) * 2016-06-06 2016-10-26 上海银帆信息科技有限公司 一种基于单点定位技术的移动机器人打印系统
CN105974922A (zh) * 2016-06-29 2016-09-28 上海诺力智能科技有限公司 一种巷道自动导引车、导引系统及其操作方法
CN106780608A (zh) * 2016-11-23 2017-05-31 北京地平线机器人技术研发有限公司 位姿信息估计方法、装置和可移动设备
CN106525053A (zh) * 2016-12-28 2017-03-22 清研华宇智能机器人（天津）有限责任公司 一种基于多传感器融合的移动机器人室内定位方法
CN107246873A (zh) * 2017-07-03 2017-10-13 哈尔滨工程大学 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法
CN107478214A (zh) * 2017-07-24 2017-12-15 杨华军 一种基于多传感器融合的室内定位方法及系统
CN107608350A (zh) * 2017-09-14 2018-01-19 南京理工大学 一种基于msp430的仓储物流机器人控制系统
CN108917759A (zh) * 2018-04-19 2018-11-30 电子科技大学 基于多层次地图匹配的移动机器人位姿纠正算法
CN109186625A (zh) * 2018-10-24 2019-01-11 北京奥特贝睿科技有限公司 智能车辆利用混合采样滤波进行精确定位的方法及系统

Also Published As

Publication number Publication date
CN110045733A (zh) 2019-07-23

Similar Documents

Publication Publication Date Title
CN110045733B (zh) 一种实时定位方法及其系统、计算机可读介质
CN110687549B (zh) 障碍物检测方法和装置
CN108256446B (zh) 用于确定道路中的车道线的方法、装置和设备
EP3875985B1 (en) Method, apparatus, computing device and computer-readable storage medium for positioning
US11527084B2 (en) Method and system for generating a bird's eye view bounding box associated with an object
CN110602647A (zh) 基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法
EP3371670A1 (en) Device and method for autonomous localisation
CN110197615B (zh) 用于生成地图的方法及装置
CN110728650B (zh) 一种基于智能终端的井盖凹陷检测方法及相关设备
CN113466822A (zh) 用于检测障碍物的方法和装置
CN108535789A (zh) 一种基于机场跑道的异物识别系统
CN114459471A (zh) 定位信息确定方法、装置、电子设备及存储介质
CN112651266A (zh) 行人检测方法和装置
CN115017578A (zh) 一种建筑的智能实测实量方法、装置、ugv及存储介质
CN112630798B (zh) 用于估计地面的方法和装置
CN111469781B (zh) 用于输出信息的方法和装置
CN114425774B (zh) 机器人行走道路的识别方法、识别设备以及存储介质
CN114049563B (zh) 带电作业环境的评估方法、作业环境评估系统及电子设备
CN113284337B (zh) 基于车辆轨迹多维数据的od矩阵计算方法及装置
CN115902839A (zh) 港口激光雷达标定方法及装置、存储介质及电子设备
CN115421486A (zh) 返航控制方法、装置、计算机可读介质及自移动设备
CN113269827B (zh) 一种实现自动化标定的方法和计算装置
CN113790725B (zh) 路径规划方法、路径规划装置、存储介质与可移动设备
CN114577198B (zh) 一种高反物体定位方法、装置及终端设备
CN115002899B (zh) 一种基于svl的视障人群乘车导航辅助方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant