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

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

【技术实现步骤摘要】
一种基于FPGA的机器人激光定位方法


[0001]本专利技术属于机器人定位
，具体涉及一种基于FPGA的机器人激光定位方法。

技术介绍

[0002]机器人定位技术是指机器人通过装备的车载传感器来感知周围的环境，从而确定自己在环境中的位置的技术。常见的机器人定位技术主要包括视觉定位以及激光定位，对应的传感器是摄像头和激光雷达，然而视觉定位会受到光强，光照等条件影响，实际的应用效果还不是特别好，而目前应用最广泛的定位技术还是通过激光雷达来定位，可广泛应用在扫地机器人，送餐机器人，消毒机器人以及工业级机器人。
[0003]由于算法精度会受到粒子数目的影响，为了达到工业级的定位精度，需要设置较多数目的粒子，这就带来的巨大的算力负担，需要中高端的CPU才能达到算法所需要的实时性的要求。但是，由于功耗、成本和物理尺寸等各方面的限制，存在无法安装这些大型CPU和GPU的情况；另一方面，机器人定位功能是所有其他机器人任务的基础，因此在所有机器人的基本功能中有着举足轻重的地位，为其他的机器人任务做铺垫。因此，亟需设计一种机器人定位算法的硬件加速器，才能够满足嵌入式边缘设备的要求。

技术实现思路

[0004]本专利技术的目的是为了解决机器人精准定位的问题，提出了一种基于FPGA的机器人激光定位方法。
[0005]本专利技术的技术方案是：一种基于FPGA的机器人激光定位方法包括以下步骤：
[0006]S1：利用里程计传感器采集机器人的里程计数据，并计算机器人各个粒子的先验位姿；
[0007]S2：根据机器人各个粒子的先验位姿计算各个粒子对应的权重得分；
[0008]S3：根据各个粒子对应的权重得分，计算机器人位姿，完成机器人定位。
[0009]进一步地，步骤S1中，计算机器人各个粒子的先验位姿的具体方法为：利用里程计传感器采集机器人上一时刻的里程计数据pold和当前时刻的里程计数据pnew，并在上一时刻的里程计数据pold和当前时刻的里程计数据pnew上叠加高斯噪声gaussian，得到机器人各个粒子的先验位姿pdelta，其计算公式为：
[0010]pdelta＝pnew
‑
pold+gaussian。
[0011]进一步地，步骤S2包括以下子步骤：
[0012]S21：根据机器人各个粒子的先验位姿计算激光点世界坐标；
[0013]S22：将各个粒子的激光点世界坐标转化为地图栅格坐标；
[0014]S23：对各个粒子的激光点地图栅格坐标求和，作为各个粒子的权重得分。
[0015]进一步地，步骤S21中，激光点世界坐标(x
t
,y
t
)的计算公式为：
[0016]x
t
＝x
t
‑1+x
k
*cosθ
t
‑1‑
y
k
*sinθ
t
‑1+z
t
*cos(θ
t
‑1+θ
t
)
[0017]y
t
＝y
t
‑1+y
k
*cosθ
t
‑1+x
k
*sinθ
t
‑1+z
t
*cos(θ
t
‑1+θ
t
)
[0018]其中，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时刻激光点的长度；
[0019]所述步骤S22中，地图栅格坐标的计算公式为：
[0020][0021][0022]其中，x
cell
表示t时刻激光点地图栅格坐标的横坐标，y
cell
表示t时刻激光点地图栅格坐标的纵坐标，x
origin
表示地图原点的世界坐标的横坐标，y
origin
表示地图原点的世界坐标的纵坐标，scale表示地图分辨率大小。
[0023]进一步地，步骤S3包括以下子步骤：
[0024]S31：筛选粒子集；
[0025]S32：在筛选后的粒子集中，根据各个粒子对应的权重得分计算机器人位姿，完成机器人定位。
[0026]进一步地，步骤S31中，筛选粒子集的具体方法为：设定权重阈值，将粒子集中权重得分中小于权重阈值的粒子删除，并复制权重得分大于权重阈值的粒子，以使粒子集的粒子个数不变，其计算公式：
[0027]b＝copy(remove(a))
[0028]其中，b表示筛选后的粒子集，a表示筛选前的粒子集，copy(
·
)表示对权重得分大于权重阈值的粒子进行复制保留的操作，remove(
·
)移除粒子集中的权重得分小于权重阈值的粒子；
[0029]所述步骤S32中，机器人位姿p的计算公式为：
[0030][0031]其中，w1,w2…
w
n
表示筛选后粒子集中各个粒子的权重得分，p1,p2…
p
n
表示筛选后粒子集中各个粒子的先验位姿。
[0032]本专利技术的有益效果是：
[0033](1)本专利技术的机器人激光定位方法能够提高定位算法的运行速度，同时具有低功耗和高能效的优势，使得算法部署在低端的嵌入式设备上成为可能，降低了设备成本，同时也降低了算法部署的难度。
[0034](2)相比于传统的CPU实现机器人激光定位算法，本专利技术的方法通过与片上ARM进行交互，完成机器人定位算法的硬件加速，降低ARM的运算负担，从而使得其可以运行更多的其他程序，也降低整个系统的功耗及成本。
附图说明
[0035]图1为机器人激光定位方法的流程图；
[0036]图2为激光定位方法对应软硬件的结构图；
[0037]图3为流水线处理示意图。
具体实施方式
[0038]下面结合附图对本专利技术的实施例作进一步的说明。
[0039]如图1所示，本专利技术提供了一种基于FPGA的机器人激光定位方法，包括以下步骤：
[0040]S1：利用里程计传感器采集机器人的里程计数据，并计算机器人各个粒子的先验位姿；
[0041]S2：根据机器人各个粒子的先验位姿计算各个粒子对应的权重得分；
[0042]S3：根据各个粒子对应的权重得分，本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于FPGA的机器人激光定位方法，其特征在于，包括以下步骤：S1：利用里程计传感器采集机器人的里程计数据，并计算机器人各个粒子的先验位姿；S2：根据机器人各个粒子的先验位姿计算各个粒子对应的权重得分；S3：根据各个粒子对应的权重得分，计算机器人位姿，完成机器人定位。2.根据权利要求1所述的基于FPGA的机器人激光定位方法，其特征在于，所述步骤S1中，计算机器人各个粒子的先验位姿的具体方法为：利用里程计传感器采集机器人上一时刻的里程计数据pold和当前时刻的里程计数据pnew，并在上一时刻的里程计数据pold和当前时刻的里程计数据pnew上叠加高斯噪声gaussian，得到机器人各个粒子的先验位姿pdelta，其计算公式为：pdelta＝pnew
‑
pold+gaussian。3.根据权利要求1所述的基于FPGA的机器人激光定位方法，其特征在于，所述步骤S2包括以下子步骤：S21：根据机器人各个粒子的先验位姿计算激光点世界坐标；S22：将各个粒子的激光点世界坐标转化为地图栅格坐标；S23：对各个粒子的激光点地图栅格坐标求和，作为各个粒子的权重得分。4.根据权利要求3所述的基于FPGA的机器人激光定位方法，其特征在于，所述步骤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

【专利技术属性】
技术研发人员：黄佳遥，周军，龙羽，徐菱，
申请(专利权)人：成都睿芯行科技有限公司，
类型：发明
国别省市：