CN113607173A - 一种基于fpga的机器人激光定位方法 - Google Patents
一种基于fpga的机器人激光定位方法 Download PDFInfo
- Publication number
- CN113607173A CN113607173A CN202111073432.1A CN202111073432A CN113607173A CN 113607173 A CN113607173 A CN 113607173A CN 202111073432 A CN202111073432 A CN 202111073432A CN 113607173 A CN113607173 A CN 113607173A
- Authority
- CN
- China
- Prior art keywords
- robot
- particle
- laser
- particles
- pose
- 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.)
- Granted
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于FPGA的机器人激光定位方法,包括以下步骤:S1:利用里程计传感器采集机器人的里程计数据,并计算机器人各个粒子的先验位姿;S2:根据机器人各个粒子的先验位姿计算各个粒子对应的权重得分;S3:根据各个粒子对应的权重得分,计算机器人位姿,完成机器人定位。本发明的机器人激光定位方法能够提高定位算法的运行速度,同时具有低功耗和高能效的优势,使得算法部署在低端的嵌入式设备上成为可能,降低了设备成本,同时也降低了算法部署的难度。
Description
技术领域
本发明属于机器人定位技术领域,具体涉及一种基于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中,激光点世界坐标(xt,yt)的计算公式为:
xt=xt-1+xk*cosθt-1-yk*sinθt-1+zt*cos(θt-1+θt)
yt=yt-1+yk*cosθt-1+xk*sinθt-1+zt*cos(θt-1+θt)
其中,xt表示t时刻激光点世界坐标的横坐标,yt表示t时刻激光点世界坐标的纵坐标,xt-1表示t-1时刻激光点世界坐标的横坐标,yt-1表示t-1时刻激光点世界坐标的纵坐标,(xk,yk)表示激光雷达相对于机器人底盘的位姿坐标,θt-1表示t-1时刻激光点的长度,zt表示t时刻激光点的角度,θt表示t时刻激光点的长度;
所述步骤S22中,地图栅格坐标的计算公式为:
其中,xcell表示t时刻激光点地图栅格坐标的横坐标,ycell表示t时刻激光点地图栅格坐标的纵坐标,xorigin表示地图原点的世界坐标的横坐标,yorigin表示地图原点的世界坐标的纵坐标,scale表示地图分辨率大小。
进一步地,步骤S3包括以下子步骤:
S31:筛选粒子集;
S32:在筛选后的粒子集中,根据各个粒子对应的权重得分计算机器人位姿,完成机器人定位。
进一步地,步骤S31中,筛选粒子集的具体方法为:设定权重阈值,将粒子集中权重得分中小于权重阈值的粒子删除,并复制权重得分大于权重阈值的粒子,以使粒子集的粒子个数不变,其计算公式:
b=copy(remove(a))
其中,b表示筛选后的粒子集,a表示筛选前的粒子集,copy(·)表示对权重得分大于权重阈值的粒子进行复制保留的操作,remove(·)移除粒子集中的权重得分小于权重阈值的粒子;
所述步骤S32中,机器人位姿p的计算公式为:
其中,w1,w2…wn表示筛选后粒子集中各个粒子的权重得分,p1,p2…pn表示筛选后粒子集中各个粒子的先验位姿。
本发明的有益效果是:
(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中,激光点世界坐标(xt,yt)的计算公式为:
xt=xt-1+xk*cosθt-1-yk*sinθt-1+zt*cos(θt-1+θt)
yt=yt-1+yk*cosθt-1+xk*sinθt-1+zt*cos(θt-1+θt)
其中,xt表示t时刻激光点世界坐标的横坐标,yt表示t时刻激光点世界坐标的纵坐标,xt-1表示t-1时刻激光点世界坐标的横坐标,yt-1表示t-1时刻激光点世界坐标的纵坐标,(xk,yk)表示激光雷达相对于机器人底盘的位姿坐标,θt-1表示t-1时刻激光点的长度,zt表示t时刻激光点的角度,θt表示t时刻激光点的长度;
所述步骤S22中,地图栅格坐标的计算公式为:
其中,xcell表示t时刻激光点地图栅格坐标的横坐标,ycell表示t时刻激光点地图栅格坐标的纵坐标,xorigin表示地图原点的世界坐标的横坐标,yorigin表示地图原点的世界坐标的纵坐标,scale表示地图分辨率大小。
在本发明实施例中,步骤S3包括以下子步骤:
S31:筛选粒子集;
S32:在筛选后的粒子集中,根据各个粒子对应的权重得分计算机器人位姿,完成机器人定位。
在本发明实施例中,步骤S31中,筛选粒子集的具体方法为:设定权重阈值,将粒子集中权重得分中小于权重阈值的粒子删除,并复制权重得分大于权重阈值的粒子,以使粒子集的粒子个数不变,其计算公式:
b=copy(remove(a))
其中,b表示筛选后的粒子集,a表示筛选前的粒子集,copy(·)表示对权重得分大于权重阈值的粒子进行复制保留的操作,remove(·)移除粒子集中的权重得分小于权重阈值的粒子;
所述步骤S32中,机器人位姿p的计算公式为:
其中,w1,w2…wn表示筛选后粒子集中各个粒子的权重得分,p1,p2…pn表示筛选后粒子集中各个粒子的先验位姿。
在本发明实施例中,本机器人激光定位方法是基于软硬件实现的,对机器人定位算法进行软硬件拆分,划分计算机整个加速器的架构,进行软硬件协同设计。整个控制***通过AXI接口将软件算法模块与硬件算法模块的计算数据进行相互传递,完成整个算法的计算流程。软件算法部分主要包括机器人先验位姿计算模块、后验位姿计算模块和粒子筛选模块。硬件算法部分包括机器人激光世界坐标计算模块、激光地图坐标转换模块、粒子权重计算模块和粒子权重生成模块。接口部分通过传输软件的粒子数据、激光数据以及距离地图数据给硬件模块,初始化硬件设备,并将硬件模块计算的结果通过接口传输给软件部分,软件部分接着计算后验位姿以及进行粒子筛选操作,从而完成整个算法的计算流程。软件算法的机器人先验位姿计算模块通过里程计传感器得到上一时刻的里程计数据与当前时刻里程计数据,估计机器人在此段时间内运行的距离,并叠加上高斯噪声,从而得到当前所有粒子的先验位姿数据。接口部分在得到了粒子的先验数据包括先验位姿数据以及先验权重数据后,并将地图数据协同激光数据以及所需要的控制信号封装成数据包的形式传送给硬件模块。通过驱动函数将传感器数据封装成结构体的形式并写入到对应的内存区域。并触发DMA模块去相应的内存地址搬运数据,传送给硬件模块,在搬运地图数据时进优化处理,由于地图数据量过大,直接将地图数据放到相应的内存区域会占用太多的内存空间,因此,每次只存储地图的一行数据,在DMA搬运完成后再搬运下一行的数据。控制信号通过写寄存器的方式完成配置。硬件加速器通过寄存器、RAM、FIFO和DDR等硬件电路来存储软件下发过来的数据,用于后续的计算。硬件加速器对地图数据采用先预计算后存储的方式,减小后续数据的计算以及读取延迟。激光数据采用顺序存储的方式,依次存储一帧激光雷达数据中的每一个激光点。粒子数据采用乒乓存储的方式,降低后续计算的复杂度。
硬件加速器待所有数据都准备好之后,便开始运算,首先通过每一个粒子的先验位姿来计算激光点的世界坐标,再通过激光地图坐标转换模块,将激光点的世界坐标转换为地图栅格坐标。从而去访问地图栅格中的数据,由于直接访问ddr延时较大,通过将栅格坐标传输给cache电路模块的方式来减小地图数据的访问延时。等待每一个粒子的全部激光点计算完毕后,求和得出该粒子所对应的权重得分。
硬件加速器的计算流程采用流水加并行的方式来进行计算,并行度可由软件算法进行配置。目前采用四并行度的方式。每当四个粒子计算完毕之后,就会触发DMA模块去将计算结果数据搬运至对应的内存区域,并触发中断让软件算法去读取相应的结果数据,从而使得软件算法进行后续计算。
如图3所示,为流水线处理示意图,激光世界坐标计算模块以及激光地图坐标计算模块采用流水线架构进行处理,同时多个计算模块并行运算。在同一时间下,可以同时运行多个步骤,加速数据的运算。
软件算法在得到每一个粒子更新后的权重数据后,进行机器人后验位姿的计算,将所有粒子的权重进行归一化运算并求位姿的加权平均值,最终得到机器人的位姿数据。粒子筛选模块通过比较粒子的权重大小来将权重小于阈值的粒子去除,并复制权重大的粒子来维持整个粒子集。软硬件协同设计从而完成整个***的功能。
本发明采用软硬件协同处理的架构,能够更好地适配不同需求,通过软件对加速器硬件进行可配置;采用并行度可配置的方式来对算法进行并行化和流水线加速。对原有算法实现了基于硬件设计的优化:第一,对距离地图采用先预计算似然,后存储的方式,减少数据延迟,同时对地图数据进行少bit量化减少存储空间的占用;第二,对粒子数据进行乒乓存储的方式减小后续计算的复杂度;第三,采用cache缓存机制,进一步减小访问DDR内存的延时,提高了***的实时性。
本发明的工作原理及过程为:首先,利用里程计传感器采集机器人的里程计数据,并计算机器人各个粒子的先验位姿;再根据机器人各个粒子的先验位姿计算各个粒子对应的权重得分;最后,根据各个粒子对应的权重得分,计算机器人位姿,完成机器人定位。
本发明的有益效果为:
(1)本发明的机器人激光定位方法能够提高定位算法的运行速度,同时具有低功耗和高能效的优势,使得算法部署在低端的嵌入式设备上成为可能,降低了设备成本,同时也降低了算法部署的难度。
(2)相比于传统的CPU实现机器人激光定位算法,本发明的方法通过与片上ARM进行交互,完成机器人定位算法的硬件加速,降低ARM的运算负担,从而使得其可以运行更多的其他程序,也降低整个***的功耗及成本。
本领域的普通技术人员将会意识到,这里所述的实施例是为了帮助读者理解本发明的原理,应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合,这些变形和组合仍然在本发明的保护范围内。
Claims (6)
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中,激光点世界坐标(xt,yt)的计算公式为:
xt=xt-1+xk*cosθt-1-yk*sinθt-1+zt*cos(θt-1+θt)
yt=yt-1+yk*cosθt-1+xk*sinθt-1+zt*cos(θt-1+θt)
其中,xt表示t时刻激光点世界坐标的横坐标,yt表示t时刻激光点世界坐标的纵坐标,xt-1表示t-1时刻激光点世界坐标的横坐标,yt-1表示t-1时刻激光点世界坐标的纵坐标,(xk,yk)表示激光雷达相对于机器人底盘的位姿坐标,θt-1表示t-1时刻激光点的长度,zt表示t时刻激光点的角度,θt表示t时刻激光点的长度;
所述步骤S22中,地图栅格坐标的计算公式为:
其中,xcell表示t时刻激光点地图栅格坐标的横坐标,ycell表示t时刻激光点地图栅格坐标的纵坐标,xorigin表示地图原点的世界坐标的横坐标,yorigin表示地图原点的世界坐标的纵坐标,scale表示地图分辨率大小。
5.根据权利要求1所述的基于FPGA的机器人激光定位方法,其特征在于,所述步骤S3包括以下子步骤:
S31:筛选粒子集;
S32:在筛选后的粒子集中,根据各个粒子对应的权重得分计算机器人位姿,完成机器人定位。
6.根据权利要求5所述的基于FPGA的机器人激光定位方法,其特征在于,所述步骤S31中,筛选粒子集的具体方法为:设定权重阈值,将粒子集中权重得分中小于权重阈值的粒子删除,并复制权重得分大于权重阈值的粒子,以使粒子集的粒子个数不变,其计算公式:
b=copy(remove(a))
其中,b表示筛选后的粒子集,a表示筛选前的粒子集,copy(·)表示对权重得分大于权重阈值的粒子进行复制保留的操作,remove(·)移除粒子集中的权重得分小于权重阈值的粒子;
所述步骤S32中,机器人位姿p的计算公式为:
其中,w1,w2...wn表示筛选后粒子集中各个粒子的权重得分,p1,p2...pn表示筛选后粒子集中各个粒子的先验位姿。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111073432.1A CN113607173B (zh) | 2021-09-14 | 2021-09-14 | 一种基于fpga的机器人激光定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111073432.1A CN113607173B (zh) | 2021-09-14 | 2021-09-14 | 一种基于fpga的机器人激光定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113607173A true CN113607173A (zh) | 2021-11-05 |
CN113607173B CN113607173B (zh) | 2023-10-20 |
Family
ID=78310475
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111073432.1A Active CN113607173B (zh) | 2021-09-14 | 2021-09-14 | 一种基于fpga的机器人激光定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113607173B (zh) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140129027A1 (en) * | 2012-11-02 | 2014-05-08 | Irobot Corporation | Simultaneous Localization And Mapping For A Mobile Robot |
CN109443351A (zh) * | 2019-01-02 | 2019-03-08 | 亿嘉和科技股份有限公司 | 一种稀疏环境下的机器人三维激光定位方法 |
CN109579849A (zh) * | 2019-01-14 | 2019-04-05 | 浙江大华技术股份有限公司 | 机器人定位方法、装置和机器人及计算机存储介质 |
CN111044036A (zh) * | 2019-12-12 | 2020-04-21 | 浙江大学 | 基于粒子滤波的远程定位方法 |
CN111765882A (zh) * | 2020-06-18 | 2020-10-13 | 浙江大华技术股份有限公司 | 激光雷达定位方法及其相关装置 |
CN112612862A (zh) * | 2020-12-24 | 2021-04-06 | 哈尔滨工业大学芜湖机器人产业技术研究院 | 一种基于点云配准的栅格地图定位方法 |
-
2021
- 2021-09-14 CN CN202111073432.1A patent/CN113607173B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140129027A1 (en) * | 2012-11-02 | 2014-05-08 | Irobot Corporation | Simultaneous Localization And Mapping For A Mobile Robot |
CN109443351A (zh) * | 2019-01-02 | 2019-03-08 | 亿嘉和科技股份有限公司 | 一种稀疏环境下的机器人三维激光定位方法 |
CN109579849A (zh) * | 2019-01-14 | 2019-04-05 | 浙江大华技术股份有限公司 | 机器人定位方法、装置和机器人及计算机存储介质 |
CN111044036A (zh) * | 2019-12-12 | 2020-04-21 | 浙江大学 | 基于粒子滤波的远程定位方法 |
CN111765882A (zh) * | 2020-06-18 | 2020-10-13 | 浙江大华技术股份有限公司 | 激光雷达定位方法及其相关装置 |
CN112612862A (zh) * | 2020-12-24 | 2021-04-06 | 哈尔滨工业大学芜湖机器人产业技术研究院 | 一种基于点云配准的栅格地图定位方法 |
Also Published As
Publication number | Publication date |
---|---|
CN113607173B (zh) | 2023-10-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109755995B (zh) | 基于ros机器人操作***的机器人自动充电对接方法 | |
US11430134B2 (en) | Hardware-based optical flow acceleration | |
CN108470190B (zh) | 基于fpga定制脉冲神经网络的图像识别方法 | |
Boikos et al. | Semi-dense SLAM on an FPGA SoC | |
CN105631798A (zh) | 低功耗便携式实时图像目标检测与跟踪***及方法 | |
WO2023272964A1 (zh) | 用于无人驾驶中激光点云定位的正态分布变换方法 | |
Tang et al. | π-soc: Heterogeneous soc architecture for visual inertial slam applications | |
Bai et al. | Pointnet on fpga for real-time lidar point cloud processing | |
CN108053385A (zh) | 一种鱼眼视频实时矫正***及方法 | |
CN109472734B (zh) | 一种基于fpga的目标检测网络及其实现方法 | |
CN111707262B (zh) | 基于最近点向量投影的点云匹配方法、介质、终端和装置 | |
CN114118181B (zh) | 一种高维回归点云配准方法、***、计算机设备及应用 | |
CN115824218A (zh) | 基于智能加速卡的地面无人平台自主导航***设计方法 | |
CN114359662B (zh) | 一种基于异构fpga和融合多分辨率的卷积神经网络的实现方法 | |
CN113607173B (zh) | 一种基于fpga的机器人激光定位方法 | |
CN113168385A (zh) | 访问动态存储器模块 | |
CN113721631A (zh) | 传感器数据处理方法、***及可读存储介质 | |
WO2020118547A1 (en) | Fpga-based acceleration using opencl on fcl in robot motion planning | |
CN112035056B (zh) | 一种基于多计算单元的并行ram访问设备及访问方法 | |
CN112997157A (zh) | 动态存储器模块中的错误校正编码 | |
CN101594364B (zh) | 高速浮点dsp处理器的以太网接口与tcp/ip协议实现技术 | |
US20210004667A1 (en) | Operation processing apparatus and operation processing method | |
CN112894794B (zh) | 人体手臂动作模仿方法、装置、终端设备及存储介质 | |
Yin et al. | FPGA-based deep learning acceleration for visual grasping control of manipulator | |
US20200177798A1 (en) | Machine Learning of Environmental Conditions to Control Positioning of Visual Sensors |
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 |