CN111708047B - 机器人定位评估方法、机器人及计算机存储介质 - Google Patents

机器人定位评估方法、机器人及计算机存储介质 Download PDF

Info

Publication number
CN111708047B
CN111708047B CN202010549935.0A CN202010549935A CN111708047B CN 111708047 B CN111708047 B CN 111708047B CN 202010549935 A CN202010549935 A CN 202010549935A CN 111708047 B CN111708047 B CN 111708047B
Authority
CN
China
Prior art keywords
robot
pose
point cloud
cloud data
laser point
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
CN202010549935.0A
Other languages
English (en)
Other versions
CN111708047A (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.)
Zhejiang Huaray Technology Co Ltd
Original Assignee
Zhejiang Huaray Technology Co Ltd
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 Zhejiang Huaray Technology Co Ltd filed Critical Zhejiang Huaray Technology Co Ltd
Priority to CN202010549935.0A priority Critical patent/CN111708047B/zh
Publication of CN111708047A publication Critical patent/CN111708047A/zh
Application granted granted Critical
Publication of CN111708047B publication Critical patent/CN111708047B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本申请公开了一种机器人定位评估方法、机器人及计算机存储介质。方法包括如下步骤：计算激光点云数据与占据栅格地图的匹配度并映射为第一评估分数；计算当前估计位姿和前一估计位姿的第一位姿变化量与当前预测位姿和前一预测位姿的第二位姿变化量的第一差异值；将第一差异值映射为第二评估分数；对第一评估分数和第二评估分数进行加权求和，得到机器人的定位估计评分；激光点云数据与先验地图匹配估算第一轨迹；实时构建子栅格地图，激光点云数据与子栅格地图匹配估算第二轨迹；计算第一轨迹和第二轨迹的第二差异值；当定位估计评分低于阈值或第二差异值高于阈值时确定机器人定位异常。通过上述方式，本申请能够提高机器人定位评估的准确性。

Description

机器人定位评估方法、机器人及计算机存储介质
技术领域
本申请涉及机器人定位技术领域，特别是涉及一种机器人定位评估方法、机器人及计算机存储介质。
背景技术
目前使用移动机器人执行任务已有了广泛的应用，例如利用机器人搬运货物、投递快递及扫地拖地等。移动机器人自主行走过程中，利用激光传感器获取根据当前环境生成的激光点云数据，并利用该激光点云数据对移动机器人进行定位，确定机器人的位姿。
当移动机器人所在的环境发生变化，例如搬运货物、人员走动导致原来的环境发生变化时，激光传感器接收到的激光点云数据发生变化，从而导致与先验地图匹配上的激光点云数据较少，会误报定位评分低，从而导致误报设备定位异常，终止原来应该执行的任务。
发明内容
本申请提供一种机器人定位评估方法、机器人及计算机存储介质，以解决相关技术中对机器人定位评估误差大的问题。
为解决上述技术问题，本申请提供一种机器人定位评估方法。该方法包括：获取当前时刻的激光点云数据、当前时刻的里程计数据和前一估计位姿；其中，前一估计位姿是对前一时刻的激光点云数据和前一时刻的里程计数据进行融合得到的；基于前一估计位姿和当前时刻的里程计数据得到当前预测位姿；使用当前时刻的激光点云数据对当前预测位姿进行修正，得到当前估计位姿；基于当前估计位姿，计算当前时刻的激光点云数据与占据栅格地图的匹配度，并将匹配度映射为第一评估分数；计算当前估计位姿与前一估计位姿的第一位姿变化量，以及计算当前预测位姿与前一预测位姿的第二位姿变化量；比较第一位姿变化量和第二位姿变化量的第一差异值，并将差异值映射为第二评估分数；使用第一预设权重对第一评估分数和第二评估分数进行加权求和，得到机器人的定位估计评分。
为解决上述技术问题，本申请提供一种机器人。该机器人包括处理器、激光传感器和里程计，其中，处理器耦接激光传感器和里程计，在工作时执行指令，以配合激光传感器和里程计实现如上述的机器人定位评估方法的步骤。
为解决上述技术问题，本申请提供一种计算机存储介质。计算机存储介质存储有计算机程序，计算机程序被执行以实现如上述的机器人定位评估方法的步骤。
本申请通过基于当前估计位姿计算当前时刻的激光点云数据与占据栅格地图匹配度，得到第一评估得分；计算当前估计位姿与前一估计位姿的第一位姿变化量，以及计算当前预测位姿与前一预测位姿的第二位姿变化量，并比较第一位姿变化量和第二位姿变化量的第一差异值，得到第二评估分数，当环境发生变化时，只要机器人定位准确，则第一位姿变化量和第二位姿变化量之间的差异不大，第二评估分数高。综合第一评估分数和第二评估分数作为机器人的定位评估得分，第二评估分数能够平衡因环境变化导致第一评估分数偏低的情况，从而降低环境变化时对机器人定位评估的误差。
附图说明
图1是本申请提供的机器人一实施例的结构示意图；
图2为本申请提供的基于仓库环境生成二维地图的示意图；
图3是本申请提供的机器人定位评估方法第一实施例的流程示意图；
图4是本申请提供的sigma映射函数的示意图；
图5是本申请提供的机器人定位评估方法第二实施例的流程示意图；
图6是本申请提供的计算机存储介质一实施例的结构示意图。
具体实施方式
为使本领域的技术人员更好地理解本发明的技术方案，下面结合附图和具体实施方式对本申请所提供的机器人定位评估方法、机器人定位评估装置及计算机存储介质做进一步详细描述。
请参阅图1，图1是本申请提供的机器人一实施例的结构示意图。
本实施例中的机器人10包括移动地盘11和激光传感器12，其中移动地盘10包括里程计13、运动控制器(图未示)、电机(图未示)、电池(图未示)、处理器(图未示)。本实施例中的机器人10可以是能够获取到激光点云数据和里程计信息的机器人，对机器人10具体包括哪些部件不作过多限定。
其中，激光传感器12是用于获取二维平面信息的传感器，用于检测周围环境的二维平面轮廓信息，采集机器人坐标系下的激光点云数据，该激光传感器可以是2D激光雷达传感器，本发明实施例不作过多限定。
里程计13可以采集机器人10角度和距离的变化量等里程计数据，可采用轮式编码器，从而能够根据前后时刻的位姿变化量和前一时刻的位姿来预测出当前时刻机器人10的位姿。
以该机器人10应用于仓储环境中为例，仓库中包括货架、工作台以及房屋支撑墙等部件，请参见图2，图2为本申请提供的基于仓库环境生成二维地图的示意图。机器人10中包括的里程计13可以估计出机器人10运动的变化量，但是由于里程计13存在误差，因而可以结合激光传感器12获取的二维平面轮廓信息。如图2所示，每一条直线表示激光传感器12发射的一束光束，基于光束激光传感器12能够感知障碍物的存在，从而扫描获取仓库的二维平面轮廓信息，进而通过二维平面轮廓信息对基于里程计13估算的位姿进行矫正，从而实现机器人10的高精度定位。
本实施例中的“位姿”表示机器人10在空间坐标系中的位置和姿态，可以用矩阵表示，如t时刻机器人10的估计位姿为xt＝[x y θ]T，x、y为机器人10在世界坐标系下的二维平面的位置，θ为机器人10在世界坐标系下的二维平面的方向。
处理器获取激光传感器12采集的机器人坐标系下的激光点云数据和里程计13采集的里程计数据，实现以下的机器人10定位评估方法的实施例：
请参阅图3，图3是本申请提供的机器人定位评估方法第一实施例的流程示意图。本实施例包括如下步骤：
S101：获取当前时刻的激光点云数据、当前时刻的里程计数据和前一估计位姿，计算当前估计位姿。
机器人在环境中移动时，激光传感器实时向环境发射激光束对当前环境进行扫描，通过飞行时间测距法计算得到机器人与环境中障碍物之间的距离，每一激光束击中障碍物的距离和该激光束相对于机器人的角度作为激光点数据，该时刻下的所有激光点数据的集合为机器人坐标系下的激光点云数据。根据该时刻下机器人预测位姿或估计位姿，可将机器人坐标系下的激光点云数据转换为世界坐标系下的激光点云数据。如无特殊说明，本实施例中所指的激光点云数据是指世界坐标系下的激光点云数据。
获取激光点云数据的方式有多种，可以采用机器人的激光传感器连续发射激光对当前环境进行扫描，连续获取激光点云数据的方式；也可以采用间隔设定时间发射激光进行扫描，间隔设定时间的获取激光点云数据方式，本申请对此不作限制。
里程计实时采集机器人角度和距离的变化量作为里程计数据。
获取当前时刻激光传感器采集的激光点云数据，当前时刻里程计采集的里程计数据，和前一时刻估计的前一估计位姿，以估计机器人当前时刻的估计位姿。
本实施例中，估计位姿可以是通过非线性卡尔曼滤波器融合激光点云数据和里程计数据得到的。具体地，非线性卡尔曼滤波器包括运动模型和观测模型。将前一时刻的估计位姿作为初始位姿，里程计数据作为控制量输入运动模型，运动模型输出机器人的预测位姿。将激光点云数据作为观测量输入观测模型对预测位姿进行修正优化，得到估计位姿。具体而言，基于预测位姿，将机器人坐标系下的激光点云数据投影到占据栅格地图，通过修正预测位姿，提高投影到占据栅格地图的激光点云数据与占据栅格地图中的障碍物匹配度，最终输出最优的估计位姿。
同样地，前一估计位姿是基于确定的初始位姿，使用非线性卡尔曼滤波器融合前一时刻激光点云数据和前一时刻里程计数据得到的。当前估计位姿是基于前一估计位姿，使用非线性卡尔曼滤波器融合当前时刻激光点云数据和当前时刻里程计数据得到的。
占据栅格地图将整个环境分为若干相同大小的栅格，对于每个栅格各指出其中是否存在障碍物。障碍物在占据栅格地图中所在的位置，概率值为1。以障碍物所在的栅格为中心，向外扩展，离障碍物越近，对应概率值越高；离障碍物越远，对应的概率值越低。其中概率值的计算，以一个均值为0，协方差为σ的正态分布进行分布。
S102：基于当前估计位姿，计算当前时刻的激光点云数据与占据栅格地图的匹配度，并将匹配度映射为第一评估分数。
本实施例中，占据栅格地图为先验地图。先验地图是指在先基于机器人所在的环境生成的全局占据栅格地图，在机器人单次执行任务的移动过程中，先验地图中栅格的概率保持不变。
基于当前估计位姿xt＝[x y θ]T，假设激光雷达的第k束激光对应的激光点数据在机器人坐标系下的坐标为[xk,sens yk,sensθk,sens]T，那么可以计算第k束激光对应的激光点在先验地图上的占据位置
Figure BDA0002542081450000051
公式表示如下：
Figure BDA0002542081450000052
计算得到第k束激光对应的激光点在先验地图上的占据位置后，获取占据位置在先验地图上最近的障碍物。具体可以通过最邻近数据关联法计算得到占据位置在先验地图上最近的障碍物。
计算占据位置与其最近的障碍物的距离作为匹配度。占据位置与其最近的障碍物的距离越小，匹配度越高；反之匹配度越低。计算占据位置与其最近的障碍物的距离公式如下：
Figure BDA0002542081450000053
式(2)中，(xm,ym)—地图中被障碍物占据的坐标。
进一步使用映射得分函数，将匹配度映射为该占据位置的得分。
qk＝ Prob(dist,σ) (3)
式(3)中，qk为第k束激光对应的激光点在先验地图上的占据位置的得分，qk服从均值为0，方差为σ的正态分布。
当前时刻的激光点云数据包括第一数量的激光点，按照本步骤的方法计算每一激光点在先验地图上的占据位置的得分。并计算每一得分之和，除以第一数量，得到第一评估分数。
Figure BDA0002542081450000061
式(4)中，Q为第一评估分数，N为第一数量。
当机器人的当前估计位姿完全准确时，激光数据能正确投影到先验地图被障碍物占据的栅格上，此时第一评估分数为100分。一般来说，当前估计位姿偏差较小时，第一评估分数高；当前估计位姿偏差较大时，第一评估分数低。
机器人所在的环境发生变化，比如新搬进来一些货物或者搬走一些货物，导致当前时刻的激光点云数据能正确投影到先验地图的数量较少，部分激光点数据因为新增的物体未能正确投影到地图上，导致第一评估分数低；或者当有一块挡板，遮住激光使得被遮住部分的激光点数据不准确，降低了能准确投影到地图上的激光点数据，此时又会错误评价第一评估分数，使得第一评估分数偏低造成较大误差。
S103：计算当前估计位姿与前一估计位姿的第一位姿变化量，以及计算当前预测位姿与前一预测位姿的第二位姿变化量。
在使用非线性卡尔曼滤波器进行里程计数据和激光点云数据进行融合时，运动模型中会使用里程计数据对当前机器人的位姿进行预测，输出预测位姿；观测模型会使用激光点云数据对预测位姿进行修正优化，输出估计位姿。当环境发生变化时，即使S102中的第一评估分数低，但只要定位准确，则观测模型当前时刻输出的当前估计位姿和前一时刻输出的前一估计位姿之间的第一位姿变化量，与运动模型当前时刻输出的当前预测位姿和前一时刻输出的前一预测位姿之间的第二位姿变化量的差异不大。
观测模型中，当前时刻输出的当前估计位姿和前一时刻输出的前一估计位姿之间的第一位姿变化量为：
Δ1＝xt-xt-1 (5)
式(5)中，Δ1表示第一位姿变化量，xt为当前估计位姿，xt-1为前一估计位姿。
运动模型中，当前时刻输出的当前预测位姿和前一时刻输出的前一预测位姿之间的第二位姿变化量为：
Δ2＝xt,odom-xt-1,odom (6)
式(6)中，Δ2表示第二位姿变化量，xt,odom为当前时刻的预测位姿，xt-1,odom为前一时刻的预测位姿。
S104：比较第一位姿变化量和第二位姿变化量的第一差异值，并将差异值映射为第二评估分数。
计算第一位姿变化量Δ1与第二位姿变化量Δ2的第一差异值x：
x＝Δ1-Δ2 (7)
根据逻辑函数sigma映射函数，将第一差异值x映射成第二评估分数score2：
Figure BDA0002542081450000071
式(8)中，βo、λo表示函数曲率和偏移量调节比例因子，E表示第一差异值的阈值。
请参阅图4，图4是本申请提供的sigma映射函数的示意图。其中，λo的值为5，βo的值为3.5，横坐标表示第一差异值，纵坐标表示第一差异值对应的得分。可以直观地看出，第一差异值越大，则第二评估分数越小；反之评估分数越高。
第一差异值的阈值可以是5cm、6cm、8cm或10cm等。当第一差异值大于E时，说明可能是当前时刻的激光点云数据误差大，则丢弃第一差异值，即第二评估分数不作为当前时刻机器人定位评估的第二维；当第一差异值小于E时，将第一差异值映射为第二评估分数，作为当前时刻机器人定位评估的第二维。
S105：使用第一预设权重对第一评估分数和第二评估分数进行加权求和，得到机器人的定位估计评分。
机器人的公式表示如下：
score＝α*score1+(1-α)*score2 (9)
式(9)中，score表示定位估计评分，α表示第一评估分数的权重，1-α表示第二评估分数的权重。
其中，α可以是0.2、0.3、0.4、0.5或0.6等。α具体可根据经验、激光传感器和里程计的精度进行设置，本申请不限于此。
定位估计评分越高，则说明当前估计位姿越准确；反之则说明当前估计位姿误差较大。
S106：判断第一预设时间段内定位估计评分低于第一阈值的次数是否大于第二阈值。
其中，第一阈值可以是50分、60分或70分等。第一阈值具体可根据经验设置，本申请不限于此。
第一预设时间段可以是基于当前时刻往前1秒、2秒或3秒等的时间段。
第二阈值可以是8次、10次、15次或20次等。
当第一预设时间段内定位估计评分低于第一阈值的次数大于第二阈值时，执行S107；当第一预设时间段内定位估计评分低于第一阈值的次数小于第二阈值时，执行S101。
通过统计第一预设时间段内定位估计评分低于第一阈值的次数，合理地限定容错范围，能够降低频繁上报异常导致机器人无法正常执行任务的风险。
S107：确定机器人定位异常，终止任务。
第一预设时间段内定位估计评分低于第一阈值的次数大于第二阈值，说明当前机器人定位异常，为降低机器人继续移动与环境中的障碍物发生碰撞的风险，及时向控制中心上报并终止当前执行的任务，停止移动。
本实施例通过基于当前估计位姿计算当前时刻的激光点云数据与占据栅格地图匹配度，得到第一评估得分；计算当前估计位姿与前一估计位姿的第一位姿变化量，以及计算当前预测位姿与前一预测位姿的第二位姿变化量，并比较第一位姿变化量和第二位姿变化量的第一差异值，得到第二评估分数，当环境发生变化时，只要机器人定位准确，则第一位姿变化量和第二位姿变化量之间的差异不大，第二评估分数高。综合第一评估分数和第二评估分数作为机器人的定位评估得分，第二评估分数能够平衡因环境变化导致第一评估分数偏低的情况，从而降低环境变化时对机器人定位评估的误差。
为了进一步提高对当前时刻机器人定位评估的准确性，还可以实时构建子栅格地图。比较机器人在先验地图中的运动轨迹和在子栅格地图中的运动轨迹差异，根据运动轨迹差异评估机器人定位是否异常。请参阅图5，图5是本申请提供的机器人定位评估方法第二实施例的流程示意图。本实施例基于机器人定位评估方法第一实施例，故相同的步骤在此不再赘述。本实施例包括如下步骤：
S201：获取当前时刻的激光点云数据、当前时刻的里程计数据和前一估计位姿，计算基于先验地图的当前估计位姿。
S202：根据基于先验地图的当前估计位姿，计算当前时刻的激光点云数据与占据栅格地图的匹配度，并将匹配度映射为第一评估分数。
在一些实施方式中，第一评估分数可以是仅基于当前时刻的激光点云数据与先验地图匹配得到的。
在另一些实施方式中，占据栅格地图可以包括先验地图和子栅格地图，第一评估分数可以是基于当前时刻的激光点云数据分别与先验地图和子栅格地图匹配得到的。
子栅格地图为实时构建的局部占据栅格地图。子栅格地图的构建与更新将在S208中进行详细描述。
基于先验地图的当前估计位姿，计算当前时刻的激光点云数据与先验地图的第一匹配度。基于子栅格地图的估计位姿，计算当前时刻的激光点云数据与子栅格地图的第二匹配度。基于子栅格地图的估计位姿是将基于先验地图的当前估计位姿作为机器人在子栅格地图中初始位姿，并对初始位姿进行修正优化得到的。
计算第一匹配度和第二匹配度的方法与S102相同，故在此不再赘述。
使用第二预设权重对第一匹配度和第二匹配度进行加权求和，得到激光点云数据与占据栅格地图的匹配度。第二预设权重可根据经验或者激光传感器进行配置，例如第一匹配度的权重为0.3、0.4、0.5或0.6等，相应的第二匹配度的权重为0.7、0.6、0.5或0.4等。
使用映射得分函数，将匹配度映射为第一评估分数。
通过融合第一匹配度和第二匹配度，可以提高第一评估分数的准确性和可靠性。具体来说，当环境发生变化时，即使当前评估位姿准确，也会造成第一匹配度偏低，而由于子栅格地图时实时构建的，与当前的环境基本一致，即使环境发生了变化第二匹配度仍然与实际一致或偏差很小。融合第一匹配度和第二匹配度能够提高第一评估分数，能够改善因环境变化造成第一评估分数低而错误确定当前定位异常的情况。
S203：计算基于先验地图的当前估计位姿与前一估计位姿的第一位姿变化量，以及计算基于先验地图的当前预测位姿和前一预测位姿的第二位姿变化量。
S204：比较第一位姿变化量和第二位姿变化量的第一差异值，并将差异值映射为第二评估分数。
S205：使用第一预设权重对第一评估分数和第二评估分数进行加权求和，得到机器人的定位估计评分。
S206：判断第一预设时间段内定位估计评分低于第一阈值的次数是否大于第二阈值。
当第一预设时间段内定位估计评分低于第一阈值的次数大于第二阈值时，执行S212；当第一预设时间段内定位估计评分低于第一阈值的次数小于第二阈值时，执行S201。
S207：基于先验地图、里程计数据和激光点云数据，估计机器人在先验地图中的第一轨迹。
基于先验地图、第二预设时间段内的里程计数据和激光点云数据，可以获得第二预设时间段内连续的基于先验地图的估计位姿。
第二预设时间段可以是指基于当前时刻往前2s、5s、10s或30s的时间段。
获取第二预设时间段内基于先验地图的估计位姿的集合，作为第一轨迹。
S208：根据激光点云数据，生成子栅格地图。
基于最近的第二数量帧激光点云数据，构建子栅格地图。具体地，在构建子栅格地图时，只将最近的第二数量帧激光点云数据构建一个子栅格地图。除了第一张子栅格地图，之后都同时构建两张子栅格地图，当前一张子栅格地图构建达到一定数量时，就构建新一张子栅格地图，直到前一张子栅格地图中包含的激光点云数据的帧数达到N，此时该子栅格地图构建完毕。构建子栅格地图的时候只保留最近的两张子栅格子图，能够降低机器人的运算量。
在构建第一张子栅格地图时，子栅格地图与先验地图的坐标已经过对准，如此能够实现后续第一轨迹和第二轨迹的比较。
将与先验地图匹配度高于第四阈值的激光点云数据插入子栅格地图中，以更新子栅格地图。与先验地图匹配度高于第四阈值时，说明该帧激光点云数据在大部分能够正确投影到先验地图的障碍物上，相对可靠。
更新子栅格地图具体如下：
更新子栅格地图中栅格的占据概率，栅格概率更新公式：
Figure BDA0002542081450000111
式(10)中，p表示认为栅格被障碍物占据的概率值，x表示栅格地图中的位置，Mold(x)、Mnew(x)表示同一栅格位置旧的概率值和新的概率值。
当占据概率值大于0.65时，认为此栅格是被障碍物占据的，此时需要动态更新此栅格障碍物周围的概率值，保证离该障碍物距离越近的栅格，得分越高；距离该障碍物越远的栅格，得分越低。
S209：基于激光点云数据和子栅格地图，估计机器人在子栅格地图中的第二轨迹。
使用基于先验地图的估计位姿作为初始位姿，利用scan to map(帧到地图)的方法将激光点云数据与最新的子栅格地图进行匹配和迭代，得到机器人基于子栅格地图中的估计位姿。优化初始位姿的方法可以是L-M(Levenberg-Marquardt)算法或G-N(Gauss-Newton)算法。
获取第二预设时间段内基于子栅格地图的估计位姿的集合，作为第二轨迹。
S210：比较第一轨迹和第二轨迹，得到第二差异值。
分别比较同一时刻下，第一轨迹中基于先验地图的估计位姿和第二轨迹中基于子栅格地图的估计位姿的差异值。
遍历第二预设时间段内所有时刻基于先验地图的估计位姿和基于子栅格地图的估计位姿的差异值，并对第二预设时间段内每一时刻对应的差异值相加求和，得到第一轨迹和第二轨迹的第二差异值。
S211：判断第二差异值是否大于第三阈值。
若第二差异值大于第三阈值，说明第一轨迹与第二轨迹差异大，可能是由于环境发生变化，导致基于先验地图的估计位姿不准确，造成机器人定位误差较大，此时执行S212。
若第二差异值小于第三阈值，执行S207。
第三阈值可根据经验或第二预设时间段内估计位姿的数量确定，例如第三阈值为50cm、60cm或70cm等，本申请不限于此。
S212：确定机器人定位异常，终止任务。
第一预设时间段内定位估计评分低于第一阈值的次数大于第二阈值，或者第二预设时间段内第一轨迹和第二轨迹的差异值大于第三阈值，说明当前机器人定位异常，为降低机器人继续移动与环境中的障碍物发生碰撞的风险，及时向控制中心上报并终止当前执行的任务，停止移动。
本实施例中，通过基于当前估计位姿计算当前时刻的激光点云数据与占据栅格地图匹配度，得到第一评估得分；计算当前估计位姿与前一估计位姿的第一位姿变化量，以及计算当前预测位姿与前一预测位姿的第二位姿变化量，并比较第一位姿变化量和第二位姿变化量的第一差异值，得到第二评估分数，当环境发生变化时，只要机器人定位准确，则第一位姿变化量和第二位姿变化量之间的差异不大，第二评估分数高。综合第一评估分数和第二评估分数作为机器人的定位评估得分，第二评估分数能够平衡因环境变化导致第一评估分数偏低的情况，从而降低环境变化时对机器人定位评估的误差。进一步地，实时构建子栅格地图，并比较机器人在先验地图中的运动轨迹和机器人在子栅格地图中的运动轨迹的差异，当差异大于第三阈值时，确定机器人定位异常，能够进一步提高机器人定位评估的准确性。
对于上述实施例的方法，其可以计算机程序的形式存在，因而本申请提出一种计算机存储介质，请参阅图6，图6是本申请提供的计算机存储介质一实施例的结构示意图。本实施例计算机存储介质400中存储有计算机程序401，其可被执行以实现上述实施例中的方法。
本实施例计算机存储介质400可以是U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等可以存储程序指令的介质，或者也可以为存储有该程序指令的服务器，该服务器可将存储的程序指令发送给其他设备运行，或者也可以自运行该存储的程序指令。
在本申请所提供的几个实施例中，应该理解到，所揭露的方法和装置，可以通过其它的方式实现。例如，以上所描述的装置实施方式仅仅是示意性的，例如，模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施方式方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅为本申请的实施方式，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (10)

1.一种机器人定位评估方法，其特征在于，所述方法包括：
获取当前时刻的激光点云数据、当前时刻的里程计数据和前一估计位姿；其中，所述前一估计位姿是对前一时刻的激光点云数据和前一时刻的里程计数据进行融合得到的；
基于所述前一估计位姿和所述当前时刻的里程计数据得到当前预测位姿；
使用所述当前时刻的激光点云数据对所述当前预测位姿进行修正，得到当前估计位姿；
基于所述当前估计位姿，计算所述当前时刻的激光点云数据与占据栅格地图的匹配度，并将所述匹配度映射为第一评估分数；
计算所述当前估计位姿与所述前一估计位姿的第一位姿变化量，以及计算当前预测位姿与前一预测位姿的第二位姿变化量；
比较所述第一位姿变化量和所述第二位姿变化量的第一差异值，并将所述第一差异值映射为第二评估分数；
使用第一预设权重对所述第一评估分数和所述第二评估分数进行加权求和，得到所述机器人的定位估计评分。
2.根据权利要求1所述的方法，其特征在于，所述使用第一预设权重对所述第一评估分数和所述第二评估分数进行加权求和，得到所述机器人的定位估计评分之后，还包括：
若第一预设时间段内所述定位估计评分低于第一阈值的次数大于第二阈值，确定所述机器人定位异常。
3.根据权利要求1所述的方法，其特征在于，所述占据栅格地图为先验地图，所述激光点云数据包括第一数量的激光点，所述基于所述当前估计位姿，计算所述激光点云数据与占据栅格地图的匹配度，并将所述匹配度映射为第一评估分数，包括：
基于所述当前估计位姿，计算每一所述激光点投影到所述先验地图上的占据位置；
获取每一所述占据位置在所述先验地图上最近的障碍物，并计算每一所述占据位置与其最近的所述障碍物的距离作为所述匹配度；
使用映射得分函数，将每一所述匹配度映射为所述占据位置的得分；
计算每一所述得分之和，除以所述第一数量，得到所述第一评估分数。
4.根据权利要求1所述的方法，其特征在于，所述占据栅格地图包括先验地图，所述使用所述当前时刻的激光点云数据对所述当前预测位姿进行修正，得到当前估计位姿之后，还包括：
基于所述先验地图、里程计数据和激光点云数据，估计所述机器人在所述先验地图中的第一轨迹；
根据所述激光点云数据，生成子栅格地图；基于所述激光点云数据和所述子栅格地图，估计所述机器人在所述子栅格地图中的第二轨迹；
比较所述第一轨迹和所述第二轨迹，得到第二差异值。
5.根据权利要求4所述的方法，其特征在于，所述比较所述第一轨迹和所述第二轨迹，得到第二差异值之后，还包括：
若所述第二差异值大于第三阈值，确定所述机器人定位异常。
6.根据权利要求4所述的方法，其特征在于，所述根据所述激光点云数据，生成子栅格地图包括：
基于最近的第二数量帧激光点云数据，构建所述子栅格地图；
将与先验地图匹配度高于第四阈值的激光点云数据插入所述子栅格地图中，以更新所述子栅格地图。
7.根据权利要求4所述的方法，其特征在于，所述基于所述先验地图、里程计数据和激光点云数据，估计所述机器人在所述先验地图中的第一轨迹，包括：
获取第二预设时间段内基于所述先验地图的估计位姿的集合，作为所述第一轨迹；所述估计位姿的集合包括所述当前估计位姿；
所述基于所述激光点云数据和所述子栅格地图，估计所述机器人在所述子栅格地图中的第二轨迹，包括：
使用所述基于先验地图的估计位姿作为初始位姿，将所述激光点云数据与所述子栅格地图进行匹配和优化，得到所述机器人基于所述子栅格地图中的估计位姿；
获取所述第二预设时间段内基于所述子栅格地图的估计位姿的集合，作为所述第二轨迹。
8.根据权利要求4所述的方法，其特征在于，所述占据栅格地图还包括所述子栅格地图；所述基于所述当前估计位姿，计算所述当前时刻的激光点云数据与占据栅格地图的匹配度，并将所述匹配度映射为第一评估分数，包括：
计算所述当前时刻的激光点云数据与所述先验地图的第一匹配度，及所述当前时刻的激光点云数据与所述子栅格地图的第二匹配度；
使用第二预设权重对所述第一匹配度和所述第二匹配度进行加权求和，得到所述激光点云数据与所述占据栅格地图的匹配度；
使用映射得分函数，将所述匹配度映射为所述第一评估分数。
9.一种机器人，其特征在于，所述机器人包括：处理器、激光传感器和里程计，其中，所述处理器耦接所述激光传感器和所述里程计，在工作时执行指令，以配合所述激光传感器和所述里程计实现如权利要求1至8任一所述的机器人定位评估方法的步骤。
10.一种计算机存储介质，其特征在于，所述计算机存储介质存储有计算机程序，所述计算机程序被执行以实现如权利要求1至8中任一项所述的机器人定位评估方法的步骤。
CN202010549935.0A 2020-06-16 2020-06-16 机器人定位评估方法、机器人及计算机存储介质 Active CN111708047B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010549935.0A CN111708047B (zh) 2020-06-16 2020-06-16 机器人定位评估方法、机器人及计算机存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010549935.0A CN111708047B (zh) 2020-06-16 2020-06-16 机器人定位评估方法、机器人及计算机存储介质

Publications (2)

Publication Number Publication Date
CN111708047A CN111708047A (zh) 2020-09-25
CN111708047B true CN111708047B (zh) 2023-02-28

Family

ID=72540681

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010549935.0A Active CN111708047B (zh) 2020-06-16 2020-06-16 机器人定位评估方法、机器人及计算机存储介质

Country Status (1)

Country Link
CN (1) CN111708047B (zh)

Families Citing this family (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112415536B (zh) * 2020-11-11 2023-07-14 南京市测绘勘察研究院股份有限公司 一种自动获取车载激光点云行车轨迹异常区域的方法
CN112462769A (zh) * 2020-11-25 2021-03-09 深圳市优必选科技股份有限公司 机器人定位方法、装置、计算机可读存储介质及机器人
CN112750161B (zh) * 2020-12-22 2023-11-03 苏州大学 用于移动机器人的地图更新方法
CN112612034B (zh) * 2020-12-24 2023-10-13 长三角哈特机器人产业技术研究院 基于激光帧和概率地图扫描的位姿匹配方法
CN112634260A (zh) * 2020-12-31 2021-04-09 上海商汤智能科技有限公司 一种地图评估的方法、装置、电子设备及存储介质
CN112835063B (zh) * 2021-01-08 2024-04-12 北京京东尚科信息技术有限公司 物体动静属性的确定方法、装置、设备及存储介质
CN112880674A (zh) * 2021-01-21 2021-06-01 深圳市镭神智能系统有限公司 一种行驶设备的定位方法、装置、设备及存储介质
WO2022186777A1 (en) * 2021-03-03 2022-09-09 Agency For Science, Technology And Research Method and system for localizing a mobile robot
CN113510703B (zh) * 2021-06-25 2022-09-16 深圳市优必选科技股份有限公司 机器人位姿的确定方法、装置、机器人及存储介质
CN113739819B (zh) * 2021-08-05 2024-04-16 上海高仙自动化科技发展有限公司 校验方法、装置、电子设备、存储介质及芯片
CN113827270B (zh) * 2021-08-24 2024-03-12 深圳市德力凯医疗设备股份有限公司 指令冲突解决方法、超声设备及计算机可读存储介质
CN113865588B (zh) * 2021-08-24 2024-03-26 上海交通大学 一种机器人定位方法和装置
CN116091593A (zh) * 2021-11-05 2023-05-09 追觅创新科技(苏州)有限公司 机器人状态的确定方法和装置、存储介质、电子装置
CN114442605B (zh) * 2021-12-16 2023-08-18 中国科学院深圳先进技术研究院 定位检测方法、装置、自主移动设备以及存储介质
CN114485662B (zh) * 2021-12-28 2024-03-08 深圳优地科技有限公司 机器人重定位方法、装置、机器人及存储介质
CN114353807B (zh) * 2022-03-21 2022-08-12 沈阳吕尚科技有限公司 一种机器人的定位方法及定位装置
CN114643579B (zh) * 2022-03-29 2024-01-16 深圳优地科技有限公司 一种机器人定位方法、装置、机器人及存储介质
CN115600823B (zh) * 2022-12-08 2023-04-14 东莞先知大数据有限公司 一种搬运货物异常检测方法、电子设备和存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106092104A (zh) * 2016-08-26 2016-11-09 深圳微服机器人科技有限公司 一种室内机器人的重定位方法及装置
CN110285806A (zh) * 2019-07-05 2019-09-27 电子科技大学 基于多次位姿校正的移动机器人快速精确定位算法
CN110530368A (zh) * 2019-08-22 2019-12-03 浙江大华技术股份有限公司 一种机器人定位方法及设备
CN110689622A (zh) * 2019-07-05 2020-01-14 电子科技大学 一种基于点云分割匹配闭环校正的同步定位与构图算法
WO2020048623A1 (en) * 2018-09-07 2020-03-12 Huawei Technologies Co., Ltd. Estimation of a pose of a robot
CN111223145A (zh) * 2020-01-03 2020-06-02 上海有个机器人有限公司 数据处理方法、系统、服务装置及其存储介质

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106092104A (zh) * 2016-08-26 2016-11-09 深圳微服机器人科技有限公司 一种室内机器人的重定位方法及装置
WO2020048623A1 (en) * 2018-09-07 2020-03-12 Huawei Technologies Co., Ltd. Estimation of a pose of a robot
CN110285806A (zh) * 2019-07-05 2019-09-27 电子科技大学 基于多次位姿校正的移动机器人快速精确定位算法
CN110689622A (zh) * 2019-07-05 2020-01-14 电子科技大学 一种基于点云分割匹配闭环校正的同步定位与构图算法
CN110530368A (zh) * 2019-08-22 2019-12-03 浙江大华技术股份有限公司 一种机器人定位方法及设备
CN111223145A (zh) * 2020-01-03 2020-06-02 上海有个机器人有限公司 数据处理方法、系统、服务装置及其存储介质

Also Published As

Publication number Publication date
CN111708047A (zh) 2020-09-25

Similar Documents

Publication Publication Date Title
CN111708047B (zh) 机器人定位评估方法、机器人及计算机存储介质
KR101362961B1 (ko) 이동 로봇 환경의 실시간 위치 측정 및 매핑을 위한 방법 및 장치
CN111536964B (zh) 机器人定位方法及装置、存储介质
EP3384360B1 (en) Simultaneous mapping and planning by a robot
US20210131823A1 (en) Method for Vehicle Environment Mapping, Corresponding System, Vehicle and Computer Program Product
CN112363158B (zh) 机器人的位姿估计方法、机器人和计算机存储介质
US20190113603A1 (en) Method for predicting a motion of an object
US11047708B2 (en) Method of estimating reliability of measurement distance of laser rangefinder, and localizating method of mobile robot using laser rangefinder
WO2018176358A1 (en) Autonomous tunnel navigation with a robotic system
CN111258316A (zh) 一种动态环境下趋势感知的机器人轨迹规划方法
US20220097231A1 (en) Obstacle avoidance control device, obstacle avoidance control system, obstacle avoidance control method, and recording medium
CN112729321A (zh) 一种机器人的扫图方法、装置、存储介质和机器人
CN111164380A (zh) 机器人的方位确定的方法、机器人的方位确定设备和机器人
CN113324542A (zh) 一种定位方法、装置、设备和存储介质
JP2019191498A (ja) 地図作成装置
CN110989619A (zh) 用于定位对象的方法、装置、设备和存储介质
CN116629106A (zh) 移动机器人运行场景的准数字孪生方法、系统、设备及介质
CN112344966B (zh) 一种定位失效检测方法、装置、存储介质及电子设备
CN110794434A (zh) 一种位姿的确定方法、装置、设备及存储介质
Ge et al. Realtime indoor localization on smartphones by multi-grained grid-based filters
CN117452429B (zh) 基于多线激光雷达的机器人定位方法及系统
CN111141290B (zh) 机器人的定位方法、定位装置、设备和存储介质
Khan et al. Dynamic Kalman filter-based velocity tracker for Intelligent vehicle
CN115914997A (zh) 养殖场大数据服务的移动个体uwb定位非视域校正系统
JP2023128778A (ja) 情報処理装置、制御方法、プログラム及び記憶媒体

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
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20201224

Address after: C10, 1199 Bin'an Road, Binjiang District, Hangzhou City, Zhejiang Province, 310051

Applicant after: ZHEJIANG HUARAY TECHNOLOGY Co.,Ltd.

Address before: No.1187 Bin'an Road, Binjiang District, Hangzhou City, Zhejiang Province

Applicant before: ZHEJIANG DAHUA TECHNOLOGY Co.,Ltd.

CB02 Change of applicant information
CB02 Change of applicant information

Address after: 310051 8 / F, building a, 1181 Bin'an Road, Binjiang District, Hangzhou City, Zhejiang Province

Applicant after: Zhejiang Huarui Technology Co.,Ltd.

Address before: C10, 1199 Bin'an Road, Binjiang District, Hangzhou City, Zhejiang Province, 310051

Applicant before: ZHEJIANG HUARAY TECHNOLOGY Co.,Ltd.

GR01 Patent grant
GR01 Patent grant