一种在高动态环境下的工业移动机器人定位方法

摘要

本发明公开了一种在高动态环境下的工业移动机器人定位方法，包括：分别采集轮式编码器、惯性测量单元和激光扫描单元的感知数据，并进行速度估计，求得速度估计对应的协方差矩阵；采用扩展卡尔曼滤波器对轮式编码器、惯性测量单元和激光扫描单元的速度估计和协方差矩阵进行融合，得到精准的融合后速度估计；对工业移动机器人进行航迹推演，并将融合后速度估计转换成第一位姿估计；采集光束似然场算法将激光扫描单元的感知数据与地图匹配定位；求得任一时刻激光扫描单元采集的激光地图的追踪激光观测质量，并进行环境变化检验；得到第二位姿估计；采用扩展卡尔曼滤波器对第一位姿估计和第二位姿估计进行融合，得到精准的第三位姿估计。

权利要求

1.一种在高动态环境下的工业移动机器人定位方法，所述工业移动机器人上安装有轮式编码器、惯性测量单元和激光扫描单元，其特征在于，包括以下步骤：分别采集轮式编码器、惯性测量单元和激光扫描单元的感知数据，并进行速度估计，求得速度估计对应的协方差矩阵；

采用扩展卡尔曼滤波器对轮式编码器、惯性测量单元和激光扫描单元的速度估计和协方差矩阵进行融合，得到精准的融合后速度估计；

对工业移动机器人进行航迹推演，并将融合后速度估计转换成第一位姿估计；所述将融合后速度估计转换成第一位姿估计，其表达式如下：时间增量：Δt＝current_t‑last_tX方向位移增量：Δx＝(vx.cosθ‑vy.sinθ).ΔtY方向位移增量：Δy＝(vx.sinθ‑vy.cosθ).Δt姿态角增量：Δθ＝vθ.ΔtX坐标：x+＝Δt

Y坐标：y+＝Δy

姿态角：θ+＝Δθ

其中，vx表示X方向线速度，vy表示Y方向线速度，vθ表示角速度，θ表示机器人的姿态角；

采集光束似然场算法将激光扫描单元的感知数据与地图匹配定位；求得任一时刻激光扫描单元采集的激光地图的追踪激光观测质量，并进行环境变化检验，得到第二位姿估计；

所述环境变化检验，包括以下步骤：求得追踪激光观测质量在时间上的变化率；

若变化率的数值小于预设的阈值，则放弃计算第二位姿估计；

若变化率的数值大于或等于预设的阈值，则求得第二位姿估计；

采用扩展卡尔曼滤波器对第一位姿估计和第二位姿估计进行融合，得到精准的第三位姿估计。

2.根据权利要求1所述的一种在高动态环境下的工业移动机器人定位方法，其特征在于，所述轮式编码器的感知数据的处理过程如下：利用轮式编码器采集获得线速度和角速度，并根据工业移动机器人预设的运动模型，求得线速度估计和角速度估计；根据线速度估计、角速度估计与实际数据的统计方式对比求得协方差矩阵。

说明书

一种在高动态环境下的工业移动机器人定位方法

技术领域

[0001] 本发明涉及高动态环境下定位技术领域，尤其是一种在高动态环境下的工业移动机器人定位方法。

背景技术

[0002] 工业移动机器人在加工车间、仓库等工厂等高动态环境工作时，其必须在准确知悉自身的实时位姿(位置与朝向)的基础之上，才能展开移动、搬运、码垛等进一步任务。故
机器人定位算法，是实现机器人自动化作业的核心技术之一。

[0003] 目前，现有技术中根据工厂作业的环境特征，工业移动机器人通常采用二维平面定位技术，即只考虑横向、纵向两个维度的位置和朝向，忽略在第三维‑竖向的相关信息。同
时，二维平面定位技术基本以使用二维激光雷达为主。

[0004] 通常的方法是激光匹配定位包括以下步骤：a首先建立一张工厂的栅格占用形式地图；b机器人运行时，把从激光雷达获取的扫描数据与该地图做匹配对比；c依据匹配结
果，得出机器人在地图中的位姿，即工厂工作空间的位姿(地图与工厂等价)。但是，其存在
以下缺陷：

[0005] 常规定位技术仅能应用在静态环境，即环境中的物体大致保持在原位置不改变。而在工厂环境中，原材料、半成品、产品货物的堆放位置时刻随加工工序而转移，再考虑到
行人、工位、加工单元布局的改变等等因素，场景变化率基本在50％以上，甚至可达80％+。
这些场景的变化并不能时刻反映到先前所建立的地图(先验地图)中。这个时候，映射真实
环境的激光扫描数据无法与地图进行匹配或错误匹配，就会出现定位丢失，进而产生严重
的安全事故。

[0006] 另外，在动态环境中人为部署路标，如张贴二维码、安装激光反光板。这样，即便是在高动态环境，机器人也能通过识别这些人工路标，准确判断自身位姿。但人工路标的添设
会涉及改造工作环境，路标维护，其损失柔性；由此可见，传统的定位已经无法满足动态环
境下的定位要求。

[0007] 在专利申请号为“201410175726.9”、专利名称为“动态环境下的移动机器人可靠定位方法”的中国发明专利，其虽然可以实现动态环境下的定位，但是，其是基于ZigBee无
线传感器节点组成的网络系统，因此，必须提前预先部署的无线传感器网络，在传感器网络
的基础上才能进行定位。那么，就存在投入成本高的问题，并且布设的节点必须足够的多。
不仅如此，现有技术中也有采用附加部署其他定位设备的方式，其同样存在设备投入成本
高的问题，例如专利申请号为“201310469003.5”、专利名称为“一种室内移动机器人定位方
法”的中国发明专利，其基于单超声波、编码器、电子罗盘，并且将超声波接收模块安装在天
花板上。

[0008] 因此，急需要提出一种逻辑简单、投入成本低廉、定位准确的高动态环境下的工业移动机器人定位方法。

发明内容

[0009] 针对上述问题，本发明的目的在于提供一种在高动态环境下的工业移动机器人定位方法，本发明采用的技术方案如下：

[0010] 一种在高动态环境下的工业移动机器人定位方法，所述工业移动机器人上安装有轮式编码器、惯性测量单元和激光扫描单元，包括以下步骤：

[0011] 分别采集轮式编码器、惯性测量单元和激光扫描单元的感知数据，并进行速度估计，求得速度估计对应的协方差矩阵；

[0012] 采用扩展卡尔曼滤波器对轮式编码器、惯性测量单元和激光扫描单元的速度估计和协方差矩阵进行融合，得到精准的融合后速度估计；

[0013] 对工业移动机器人进行航迹推演，并将融合后速度估计转换成第一位姿估计；

[0014] 采集光束似然场算法将激光扫描单元的感知数据与地图匹配定位；求得任一时刻激光扫描单元采集的激光地图的追踪激光观测质量，并进行环境变化检验；得到第二位姿
估计；

[0015] 采用扩展卡尔曼滤波器对第一位姿估计和第二位姿估计进行融合，得到精准的第三位姿估计。

[0016] 进一步地，所述轮式编码器的感知数据的处理过程如下：

[0017] 利用轮式编码器采集获得线速度和角速度，并根据工业移动机器人预设的运动模型，求得线速度估计和角速度估计；根据线速度估计、角速度估计与实际数据的统计方式对
比求得协方差矩阵。

[0018] 进一步地，所述环境变化检验，包括以下步骤：

[0019] 求得追踪激光观测质量在时间上的变化率；

[0020] 若变化率的数值小于预设的阈值，则放弃计算第二位姿估计；

[0021] 若变化率的数值大于或等于预设的阈值，则求得第二位姿估计。

[0022] 优选地，所述将融合后速度估计转换成第一位姿估计，其表达式如下：

[0023] 时间增量：Δt＝current_t‑last_t

[0024] X方向位移增量：Δx＝(vx.cosθ‑vy.sinθ).Δt

[0025] Y方向位移增量：Δy＝(vx.sinθ‑vy.cosθ).Δt

[0026] 姿态角增量：Δθ＝vθ.Δt

[0027] X坐标：x+＝Δt

[0028] Y坐标：y+＝Δy

[0029] 姿态角：θ+＝Δθ

[0030] 其中，vx表示X方向线速度，vy表示Y方向线速度，vθ表示角速度，θ表示机器人的姿态角。

[0031] 与现有技术相比，本发明具有以下有益效果：

[0032] (1)本发明巧妙地采集轮式编码器、惯性测量单元和激光扫描单元的感知数据，依据三种独立感知源分别计算得出各自对于机器人的速度估计和协方差矩阵(即不确定度)，
为第一次融合提供保障，并且获得精准的定位信息；

[0033] (2)本发明巧妙地采用了两次扩展卡尔曼滤波器融合，第一次融合是采用多个高斯信号相乘会得到一个方差更小的高斯分布，即信号增多，不确定度减少，在卡尔曼滤波器
系统的更新模块，从而得到一个比以上三者单独要更加精确的速度估计；而第二次融合，综
合考虑了环境的变化情况，以提高工业移动机器人在高动态环境下的适应能力；

[0034] (3)本发明通过激光数据与地图匹配定位，并使用光束似然场算法，即激光扫描到周围障碍物的距离与方向，是否如与地图吻合，得到一个依赖地图的较准确的位姿估计。

[0035] (4)本发明无需在高动态环境下预先布设其他附属定位设备，其设备投入成本低廉，并且定位准确。

[0036] 综上所述，本发明具有定位准确、适应能力强、设备投入成本低等优点，在高动态环境下定位技术领域具有很高的实用价值和推广价值。

附图说明

[0037] 为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需使用的附图作简单介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对保护
范围的限定，对于本领域技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附
图获得其他相关的附图。

[0038] 图1为本发明的逻辑流程图。

具体实施方式

[0039] 为使本申请的目的、技术方案和优点更为清楚，下面结合附图和实施例对本发明作进一步说明，本发明的实施方式包括但不限于下列实施例。基于本申请中的实施例，本领
域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请
保护的范围。

[0040] 实施例

[0041] 如图1所示，本实施例提供了一种在高动态环境下的工业移动机器人定位方法，所述工业移动机器人上安装有轮式编码器、惯性测量单元和激光扫描单元，所述定位方法包
括以下步骤：

[0042] 第一步：感知数据获取并转化为速度估计(即线速度、角速度、及不确定程度)；

[0043] (1)利用轮式编码器采集获得电机转动信息，并根据工业移动机器人预设的运动模型，计算线速度估计和角速度估计；根据速度估计与实际速度以统计方式计算得协方差
矩阵；

[0044] (2)惯性测量单元IMU包括三轴加速度计、三轴陀螺仪、三轴磁力计。其通过常规九轴数据融合与校准手段，得到角速度、线加速度均值估计及不确定度。

[0045] (3)激光扫描单元：采用rf2o方法，该方法快速而精确，从连续范围扫描中估计激光雷达的平面运动。

[0046] 第二步：采用扩展卡尔曼滤波器对轮式编码器、惯性测量单元和激光扫描单元的速度估计和协方差矩阵进行融合，得到精准的融合后的速度估计，在此，采用两个高斯信号
相乘会得到一个方差更小的高斯分布，即信号较多，不确定度较少。

[0047] 第三步：对工业移动机器人进行航迹推演，并将融合后的速度估计转换成第一位姿估计，其达式如下：

[0048] 时间增量：Δt＝current_t‑last_t

[0049] X方向位移增量：Δx＝(vx.cosθ‑vy.sinθ).Δt

[0050] Y方向位移增量：Δy＝(vx.sinθ‑vy.cosθ).Δt

[0051] 姿态角增量：Δθ＝vθ.Δt

[0052] X坐标：x+＝Δt

[0053] Y坐标：y+＝Δy

[0054] 姿态角：θ+＝Δθ

[0055] 其中，vx表示X方向线速度，vy表示Y方向线速度，vθ表示角速度，θ表示机器人的姿态角。

[0056] 第四步，采集光束似然场算法将激光扫描单元的感知数据与地图匹配定位；求得任一时刻激光扫描单元采集的激光地图的追踪激光观测质量，并进行环境变化检验；得到
第二速度估计；

[0057] 在本实施例中，如果在机器人附近的环境变化较大，那么在这个局部区域内的激光地图匹配方式就是不可采用的；本实施例利用每时每刻计算追踪激光观测质量Q，其表达
式为：

[0058] Q＝P(lidar_range|Robot_Pose,Map)

[0059] 即在当前机器人位姿与先验地图的情况下，获得该激光数值的概率。

[0060] 具体来说，可以用高斯分布来实现，Q＝Gaussian(dist,σ)

[0061] 其中， Rx表示机器人X坐标，Ry表示机器人Y坐标，mz表示实际激光数值，σ多次实验测得的标准差。

[0062] 进一步地，本实施例的追踪激光观测质量在时间上的变化率，且数值指数化处理：

[0063] (1)如果该值低于预设阈值，说明机器人进入观测质量很差的某一局部区域，也就是说周围环境相交地图发生了很大变化，那么地图并不能真实反映周围情况，激光地图匹
配定位不可信，则放弃计算第二速度估计。

[0064] (2)如果该值持续保持在预设阈值之上，接收激光地图匹配定位做第二次的位姿层融合。

[0065] 第五步，采用扩展卡尔曼滤波器对第一速度估计和第二速度估计进行融合，得到精准的第三速度估计，即便环境发了某种可接收程度的变化，根据第三位姿估计、轮式编码
器、惯性测量单元和激光扫描单元进行准确定位导航。

[0066] 上述实施例仅为本发明的优选实施例，并非对本发明保护范围的限制，但凡采用本发明的设计原理，以及在此基础上进行非创造性劳动而作出的变化，均应属于本发明的
保护范围之内。