CN113607173B - 一种基于fpga的机器人激光定位方法 - Google Patents

Abstract

本发明公开了一种基于FPGA的机器人激光定位方法，包括以下步骤：S1：利用里程计传感器采集机器人的里程计数据，并计算机器人各个粒子的先验位姿；S2：根据机器人各个粒子的先验位姿计算各个粒子对应的权重得分；S3：根据各个粒子对应的权重得分，计算机器人位姿，完成机器人定位。本发明的机器人激光定位方法能够提高定位算法的运行速度，同时具有低功耗和高能效的优势，使得算法部署在低端的嵌入式设备上成为可能，降低了设备成本，同时也降低了算法部署的难度。

Description

一种基于FPGA的机器人激光定位方法

技术领域

本发明属于机器人定位技术领域，具体涉及一种基于FPGA的机器人激光定位方法。

背景技术

机器人定位技术是指机器人通过装备的车载传感器来感知周围的环境，从而确定自己在环境中的位置的技术。常见的机器人定位技术主要包括视觉定位以及激光定位，对应的传感器是摄像头和激光雷达，然而视觉定位会受到光强，光照等条件影响，实际的应用效果还不是特别好，而目前应用最广泛的定位技术还是通过激光雷达来定位，可广泛应用在扫地机器人，送餐机器人，消毒机器人以及工业级机器人。

由于算法精度会受到粒子数目的影响，为了达到工业级的定位精度，需要设置较多数目的粒子，这就带来的巨大的算力负担，需要中高端的CPU才能达到算法所需要的实时性的要求。但是，由于功耗、成本和物理尺寸等各方面的限制，存在无法安装这些大型CPU和GPU的情况；另一方面，机器人定位功能是所有其他机器人任务的基础，因此在所有机器人的基本功能中有着举足轻重的地位，为其他的机器人任务做铺垫。因此，亟需设计一种机器人定位算法的硬件加速器，才能够满足嵌入式边缘设备的要求。

发明内容

本发明的目的是为了解决机器人精准定位的问题，提出了一种基于FPGA的机器人激光定位方法。

本发明的技术方案是：一种基于FPGA的机器人激光定位方法包括以下步骤：

S1：利用里程计传感器采集机器人的里程计数据，并计算机器人各个粒子的先验位姿；

S2：根据机器人各个粒子的先验位姿计算各个粒子对应的权重得分；

S3：根据各个粒子对应的权重得分，计算机器人位姿，完成机器人定位。

进一步地，步骤S1中，计算机器人各个粒子的先验位姿的具体方法为：利用里程计传感器采集机器人上一时刻的里程计数据pold和当前时刻的里程计数据pnew，并在上一时刻的里程计数据pold和当前时刻的里程计数据pnew上叠加高斯噪声gaussian，得到机器人各个粒子的先验位姿pdelta，其计算公式为：

pdelta＝pnew-pold+gaussian。

进一步地，步骤S2包括以下子步骤：

S21：根据机器人各个粒子的先验位姿计算激光点世界坐标；

S22：将各个粒子的激光点世界坐标转化为地图栅格坐标；

S23：对各个粒子的激光点地图栅格坐标求和，作为各个粒子的权重得分。

进一步地，步骤S21中，激光点世界坐标(x_t,y_t)的计算公式为：

x_t＝x_t-1+x_k*cosθ_t-1-y_k*sinθ_t-1+z_t*cos(θ_t-1+θ_t)

y_t＝y_t-1+y_k*cosθ_t-1+x_k*sinθ_t-1+z_t*cos(θ_t-1+θ_t)

其中，x_t表示t时刻激光点世界坐标的横坐标，y_t表示t时刻激光点世界坐标的纵坐标，x_t-1表示t-1时刻激光点世界坐标的横坐标，y_t-1表示t-1时刻激光点世界坐标的纵坐标，(x_k,y_k)表示激光雷达相对于机器人底盘的位姿坐标，θ_t-1表示t-1时刻激光点的长度，z_t表示t时刻激光点的角度，θ_t表示t时刻激光点的长度；

所述步骤S22中，地图栅格坐标的计算公式为：

其中，x_cell表示t时刻激光点地图栅格坐标的横坐标，y_cell表示t时刻激光点地图栅格坐标的纵坐标，x_origin表示地图原点的世界坐标的横坐标，y_origin表示地图原点的世界坐标的纵坐标，scale表示地图分辨率大小。

进一步地，步骤S3包括以下子步骤：

S31：筛选粒子集；

S32：在筛选后的粒子集中，根据各个粒子对应的权重得分计算机器人位姿，完成机器人定位。

进一步地，步骤S31中，筛选粒子集的具体方法为：设定权重阈值，将粒子集中权重得分中小于权重阈值的粒子删除，并复制权重得分大于权重阈值的粒子，以使粒子集的粒子个数不变，其计算公式：

b＝copy(remove(a))

其中，b表示筛选后的粒子集，a表示筛选前的粒子集，copy(·)表示对权重得分大于权重阈值的粒子进行复制保留的操作，remove(·)移除粒子集中的权重得分小于权重阈值的粒子；

所述步骤S32中，机器人位姿p的计算公式为：

其中，w₁,w₂…w_n表示筛选后粒子集中各个粒子的权重得分，p₁,p₂…p_n表示筛选后粒子集中各个粒子的先验位姿。

本发明的有益效果是：

(1)本发明的机器人激光定位方法能够提高定位算法的运行速度，同时具有低功耗和高能效的优势，使得算法部署在低端的嵌入式设备上成为可能，降低了设备成本，同时也降低了算法部署的难度。

(2)相比于传统的CPU实现机器人激光定位算法，本发明的方法通过与片上ARM进行交互，完成机器人定位算法的硬件加速，降低ARM的运算负担，从而使得其可以运行更多的其他程序，也降低整个系统的功耗及成本。

附图说明

图1为机器人激光定位方法的流程图；

图2为激光定位方法对应软硬件的结构图；

图3为流水线处理示意图。

具体实施方式

下面结合附图对本发明的实施例作进一步的说明。

如图1所示，本发明提供了一种基于FPGA的机器人激光定位方法，包括以下步骤：

S1：利用里程计传感器采集机器人的里程计数据，并计算机器人各个粒子的先验位姿；

S2：根据机器人各个粒子的先验位姿计算各个粒子对应的权重得分；

S3：根据各个粒子对应的权重得分，计算机器人位姿，完成机器人定位。

在本发明实施例中，步骤S1中，计算机器人各个粒子的先验位姿的具体方法为：利用里程计传感器采集机器人上一时刻的里程计数据pold和当前时刻的里程计数据pnew，并在上一时刻的里程计数据pold和当前时刻的里程计数据pnew上叠加高斯噪声gaussian，得到机器人各个粒子的先验位姿pdelta，其计算公式为：

pdelta＝pnew-pold+gaussian。

在本发明实施例中，步骤S2包括以下子步骤：

S21：根据机器人各个粒子的先验位姿计算激光点世界坐标；

S22：将各个粒子的激光点世界坐标转化为地图栅格坐标；

S23：对各个粒子的激光点地图栅格坐标求和，作为各个粒子的权重得分。

在本发明实施例中，步骤S21中，激光点世界坐标(x_t,y_t)的计算公式为：

x_t＝x_t-1+x_k*cosθ_t-1-y_k*sinθ_t-1+z_t*cos(θ_t-1+θ_t)

y_t＝y_t-1+y_k*cosθ_t-1+x_k*sinθ_t-1+z_t*cos(θ_t-1+θ_t)

其中，x_t表示t时刻激光点世界坐标的横坐标，y_t表示t时刻激光点世界坐标的纵坐标，x_t-1表示t-1时刻激光点世界坐标的横坐标，y_t-1表示t-1时刻激光点世界坐标的纵坐标，(x_k,y_k)表示激光雷达相对于机器人底盘的位姿坐标，θ_t-1表示t-1时刻激光点的长度，z_t表示t时刻激光点的角度，θ_t表示t时刻激光点的长度；

所述步骤S22中，地图栅格坐标的计算公式为：

其中，x_cell表示t时刻激光点地图栅格坐标的横坐标，y_cell表示t时刻激光点地图栅格坐标的纵坐标，x_origin表示地图原点的世界坐标的横坐标，y_origin表示地图原点的世界坐标的纵坐标，scale表示地图分辨率大小。

在本发明实施例中，步骤S3包括以下子步骤：

S31：筛选粒子集；

S32：在筛选后的粒子集中，根据各个粒子对应的权重得分计算机器人位姿，完成机器人定位。

在本发明实施例中，步骤S31中，筛选粒子集的具体方法为：设定权重阈值，将粒子集中权重得分中小于权重阈值的粒子删除，并复制权重得分大于权重阈值的粒子，以使粒子集的粒子个数不变，其计算公式：

b＝copy(remove(a))

其中，b表示筛选后的粒子集，a表示筛选前的粒子集，copy(·)表示对权重得分大于权重阈值的粒子进行复制保留的操作，remove(·)移除粒子集中的权重得分小于权重阈值的粒子；

所述步骤S32中，机器人位姿p的计算公式为：

其中，w₁,w₂…w_n表示筛选后粒子集中各个粒子的权重得分，p₁,p₂…p_n表示筛选后粒子集中各个粒子的先验位姿。

在本发明实施例中，本机器人激光定位方法是基于软硬件实现的，对机器人定位算法进行软硬件拆分，划分计算机整个加速器的架构，进行软硬件协同设计。整个控制系统通过AXI接口将软件算法模块与硬件算法模块的计算数据进行相互传递，完成整个算法的计算流程。软件算法部分主要包括机器人先验位姿计算模块、后验位姿计算模块和粒子筛选模块。硬件算法部分包括机器人激光世界坐标计算模块、激光地图坐标转换模块、粒子权重计算模块和粒子权重生成模块。接口部分通过传输软件的粒子数据、激光数据以及距离地图数据给硬件模块，初始化硬件设备，并将硬件模块计算的结果通过接口传输给软件部分，软件部分接着计算后验位姿以及进行粒子筛选操作，从而完成整个算法的计算流程。软件算法的机器人先验位姿计算模块通过里程计传感器得到上一时刻的里程计数据与当前时刻里程计数据，估计机器人在此段时间内运行的距离，并叠加上高斯噪声，从而得到当前所有粒子的先验位姿数据。接口部分在得到了粒子的先验数据包括先验位姿数据以及先验权重数据后，并将地图数据协同激光数据以及所需要的控制信号封装成数据包的形式传送给硬件模块。通过驱动函数将传感器数据封装成结构体的形式并写入到对应的内存区域。并触发DMA模块去相应的内存地址搬运数据，传送给硬件模块，在搬运地图数据时进优化处理，由于地图数据量过大，直接将地图数据放到相应的内存区域会占用太多的内存空间，因此，每次只存储地图的一行数据，在DMA搬运完成后再搬运下一行的数据。控制信号通过写寄存器的方式完成配置。硬件加速器通过寄存器、RAM、FIFO和DDR等硬件电路来存储软件下发过来的数据，用于后续的计算。硬件加速器对地图数据采用先预计算后存储的方式，减小后续数据的计算以及读取延迟。激光数据采用顺序存储的方式，依次存储一帧激光雷达数据中的每一个激光点。粒子数据采用乒乓存储的方式，降低后续计算的复杂度。

硬件加速器待所有数据都准备好之后，便开始运算，首先通过每一个粒子的先验位姿来计算激光点的世界坐标，再通过激光地图坐标转换模块，将激光点的世界坐标转换为地图栅格坐标。从而去访问地图栅格中的数据，由于直接访问ddr延时较大，通过将栅格坐标传输给cache电路模块的方式来减小地图数据的访问延时。等待每一个粒子的全部激光点计算完毕后，求和得出该粒子所对应的权重得分。

硬件加速器的计算流程采用流水加并行的方式来进行计算，并行度可由软件算法进行配置。目前采用四并行度的方式。每当四个粒子计算完毕之后，就会触发DMA模块去将计算结果数据搬运至对应的内存区域，并触发中断让软件算法去读取相应的结果数据，从而使得软件算法进行后续计算。

如图3所示，为流水线处理示意图，激光世界坐标计算模块以及激光地图坐标计算模块采用流水线架构进行处理，同时多个计算模块并行运算。在同一时间下，可以同时运行多个步骤，加速数据的运算。

软件算法在得到每一个粒子更新后的权重数据后，进行机器人后验位姿的计算，将所有粒子的权重进行归一化运算并求位姿的加权平均值，最终得到机器人的位姿数据。粒子筛选模块通过比较粒子的权重大小来将权重小于阈值的粒子去除，并复制权重大的粒子来维持整个粒子集。软硬件协同设计从而完成整个系统的功能。

本发明采用软硬件协同处理的架构，能够更好地适配不同需求，通过软件对加速器硬件进行可配置；采用并行度可配置的方式来对算法进行并行化和流水线加速。对原有算法实现了基于硬件设计的优化：第一，对距离地图采用先预计算似然，后存储的方式，减少数据延迟，同时对地图数据进行少bit量化减少存储空间的占用；第二，对粒子数据进行乒乓存储的方式减小后续计算的复杂度；第三，采用cache缓存机制，进一步减小访问DDR内存的延时，提高了系统的实时性。

本发明的工作原理及过程为：首先，利用里程计传感器采集机器人的里程计数据，并计算机器人各个粒子的先验位姿；再根据机器人各个粒子的先验位姿计算各个粒子对应的权重得分；最后，根据各个粒子对应的权重得分，计算机器人位姿，完成机器人定位。

本发明的有益效果为：

(1)本发明的机器人激光定位方法能够提高定位算法的运行速度，同时具有低功耗和高能效的优势，使得算法部署在低端的嵌入式设备上成为可能，降低了设备成本，同时也降低了算法部署的难度。

(2)相比于传统的CPU实现机器人激光定位算法，本发明的方法通过与片上ARM进行交互，完成机器人定位算法的硬件加速，降低ARM的运算负担，从而使得其可以运行更多的其他程序，也降低整个系统的功耗及成本。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (4)

1.一种基于FPGA的机器人激光定位方法，其特征在于，包括以下步骤：

S1：利用里程计传感器采集机器人的里程计数据，并计算机器人各个粒子的先验位姿；

S2：根据机器人各个粒子的先验位姿计算各个粒子对应的权重得分；

S3：根据各个粒子对应的权重得分，计算机器人位姿，完成机器人定位；

所述步骤S2包括以下子步骤：

S21：根据机器人各个粒子的先验位姿计算激光点世界坐标；

S22：将各个粒子的激光点世界坐标转化为地图栅格坐标；

S23：对各个粒子的激光点地图栅格坐标求和，作为各个粒子的权重得分；

所述步骤S21中，激光点世界坐标(x_t，y_t)的计算公式为：

x_t＝x_t-1+x_k*cosθ_t-1-y_k*sinθ_t-1+z_t*cos(θ_t-1+θ_t)

y_t＝y_t-1+y_k*cosθ_t-1+x_k*sinθ_t-1+z_t*cos(θ_t-1+θ_t)

其中，x_t表示t时刻激光点世界坐标的横坐标，y_t表示t时刻激光点世界坐标的纵坐标，x_t-1表示t-1时刻激光点世界坐标的横坐标，y_t-1表示t-1时刻激光点世界坐标的纵坐标，(x_k，y_k)表示激光雷达相对于机器人底盘的位姿坐标，θ_t-1表示t-1时刻激光点的长度，z_t表示t时刻激光点的角度，θ_t表示t时刻激光点的长度；

所述步骤S22中，地图栅格坐标的计算公式为：

其中，x_cell表示t时刻激光点地图栅格坐标的横坐标，y_cell表示t时刻激光点地图栅格坐标的纵坐标，x_origin表示地图原点的世界坐标的横坐标，y_origin表示地图原点的世界坐标的纵坐标，scale表示地图分辨率大小。

2.根据权利要求1所述的基于FPGA的机器人激光定位方法，其特征在于，所述步骤S1中，计算机器人各个粒子的先验位姿的具体方法为：利用里程计传感器采集机器人上一时刻的里程计数据pold和当前时刻的里程计数据pnew，并在上一时刻的里程计数据pold和当前时刻的里程计数据pnew上叠加高斯噪声gaussian，得到机器人各个粒子的先验位姿pdelta，其计算公式为：

pdelta＝pnew-pold+gaussian。

3.根据权利要求1所述的基于FPGA的机器人激光定位方法，其特征在于，所述步骤S3包括以下子步骤：

S31：筛选粒子集；

S32：在筛选后的粒子集中，根据各个粒子对应的权重得分计算机器人位姿，完成机器人定位。

4.根据权利要求3所述的基于FPGA的机器人激光定位方法，其特征在于，所述步骤S31中，筛选粒子集的具体方法为：设定权重阈值，将粒子集中权重得分中小于权重阈值的粒子删除，并复制权重得分大于权重阈值的粒子，以使粒子集的粒子个数不变，其计算公式：

b＝copy(remove(a))

其中，b表示筛选后的粒子集，a表示筛选前的粒子集，copy(·)表示对权重得分大于权重阈值的粒子进行复制保留的操作，remove(·)移除粒子集中的权重得分小于权重阈值的粒子；

所述步骤S32中，机器人位姿p的计算公式为：

其中，w₁，w₂...w_n表示筛选后粒子集中各个粒子的权重得分，p₁，p₂...p_n表示筛选后粒子集中各个粒子的先验位姿。