CN106873447A - 一种基于fpga的机器人模型算法实现方法 - Google Patents

一种基于fpga的机器人模型算法实现方法 Download PDF

Info

Publication number
CN106873447A
CN106873447A CN201710016758.8A CN201710016758A CN106873447A CN 106873447 A CN106873447 A CN 106873447A CN 201710016758 A CN201710016758 A CN 201710016758A CN 106873447 A CN106873447 A CN 106873447A
Authority
CN
China
Prior art keywords
module
robot model
fpga
fifo
algorithm
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
Application number
CN201710016758.8A
Other languages
English (en)
Other versions
CN106873447B (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.)
Nankai University
Original Assignee
Nankai University
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 Nankai University filed Critical Nankai University
Priority to CN201710016758.8A priority Critical patent/CN106873447B/zh
Publication of CN106873447A publication Critical patent/CN106873447A/zh
Application granted granted Critical
Publication of CN106873447B publication Critical patent/CN106873447B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/04—Programme control other than numerical control, i.e. in sequence controllers or logic controllers
    • G05B19/042—Programme control other than numerical control, i.e. in sequence controllers or logic controllers using digital processors
    • G05B19/0423—Input/output
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/20—Pc systems
    • G05B2219/23—Pc programming
    • G05B2219/23005—Expert design system, uses modeling, simulation, to control design process

Abstract

一种基于FPGA的机器人模型算法实现方法，通过FPGA的DMA IP核从上层处理器获得串行数据，通过串并转换模块获得主手操作臂三个旋转关节的角度信息，然后经过FIFO缓存传给机器人模型计算IP核，经过计算得到从手操作臂的三个旋转关节的角度信息，通过并串转换模块得到串行数据，最后通过DMA IP核回传给上层的处理器。本发明借助System Generator工具实现机器人模型计算模块IP核设计，对所设计的机器人模型进行仿真，以验证所提方法的可行性，本发明提高了机器人模型算法的处理速度，满足高精度测量的同时，能达到高速实时的需求，具有低功耗、小型化、高速等特点，可应用于实施高精度的机器人控制领域。

Description

一种基于FPGA的机器人模型算法实现方法
技术领域
本发明涉及一种基于FPGA的机器人模型算法的实现方法，特别涉及一种用于主从异构机器人模型计算的FPGA实现技术，属于机器人控制领域。
背景技术
随着机器人技术的蓬勃发展，机器人正在广泛地应用于汽车、电子、食品等行业代替人类从事重复的劳动。当需要在一些人类难以到达或者危险的环境中完成复杂任务时，普通机器人往往难以胜任，这时主从控制机器人应运而生，主端机器人把操作者的控制命令通过一定的通信手段传输给位于特殊或者危险环境中的从端机器人。研究能广泛应用于此类环境的主从控制机器人是当前机器人领域的一个热门话题。
主从控制机器人模型包括主手臂的机器人正运动学和从手操作臂的逆运动学，机器人运动学计算涉及到非线性计算、线性计算，运算量大，用通用计算机或DSP等进行计算时，实时性很难保证，用高性能计算机处来处理成本又非常高。FPGA虽然在速度上与DSP略有差距，但能实现并行结构。最新推出的FPGA器件中,不仅集成有丰富的可配置逻辑块资源,还包括大量面向计算密集应用的DSP单元、块状RAM和高速串行通信单元。因此选择可编程的逻辑器件进行数字信号处理是当今世界的发展趋势。
发明内容
本发明的目的是为了克服上述现有技术的缺陷，能够在实现机器人模型算法时既能保证实时性又不会成本太高，提出了一种基于FPGA的机器人模型算法的实现方法，该模块全部由硬件描述语言实现，可以提升数据处理运行速度和稳定性以及数据的计算效率，节省开发成本。
本发明的目的是通过以下技术方案实现的。
本发明的一种基于FPGA的机器人模型算法实现方法，其实现平台是FPGA，采用硬件描述语言实现，模块输入为上层处理器下传的串行数据流，机器人模型算法模块包括DMA子模块、串并转换子模块、FIFO子模块、机器人模型计算模块、算法使能模块、并串转换模块；首先通过FPGA内的DMA子模块获得上层处理器的主手操作臂各关节的角度信息的串行数据流，并将串行数据流传给FIFO模块，之后串并转换子模块对串行数据流转为并行数据流传入FIFO子模块，之后送入机器人模型计算模块计算从手操作臂各个关节的角度信息，将得到的计算结果送入FIFO进行缓存，同时主手操作臂角度信息的FIFO和从手操作臂角度信息的FIFO的full信号送入算法使能模块用来确定机器人模型计算模块使能时间，最后经过并串转换模块将并行数据转换为串行数据，并且通过DMA模块回传给上层处理器。
上述一种基于FPGA的机器人模型算法实现方法，其步骤如下：
1)通过FPGA内的DMA子模块获得上层处理器的主手操作臂各关节的角度信息的串行数据流，并将串行数据流传给FIFO模块；
2)经过FIFO子模块缓存后送入串并转换子模块；
3)串并转换子模块对步骤2)生成的串行数据流转为并行数据流传入FIFO子模块；
4)经过FIFO子模块缓存步骤3)的数据后送入机器人模型计算子模块；
5)机器人模型计算子模块利用System Generator工具建立，其实现的功能为实现主手操作臂正运动学和从手操作臂逆运动学，将计算得到的从手操作臂的关节角度信息送入FIFO子模块；
6)经过FIFO子模块缓存步骤5)的数据后送入并串转化子模块；
7)步骤4)和步骤6)的FIFO的full信号送入算法使能模块；
8)算法使能模块用来控制算法计算模块的使能；
9)并串转换模块对步骤6)的并行数据进行处理得到串行数据流，并传入DMA模块；
10)通过DMA模块将步骤9)的数据回传给上层处理器；
上述DMA模块用于实现上层处理器和FPGA内的大数据交互，能大幅度提高数据交互的速度，从而提高系统的实时性。
上述FIFO模块用于对数据缓存，对提高系统的性能有很大的作用，并且能防止数据的丢失。
上述机器人模型计算模块包括反余弦计算模块、延时子模块、基本四则运算模块，设计步骤包括：在Matlab/Simulink软件环境中设计机器人模型计算模块的Simulink模型文件；完成模型文件设计后，利用Simulink的图形仿真工具，对设计的模型文件进行仿真；在模型文件的设计时添加System Generator模块，利用该模块，将会把Simulink中设计好的.mdl文件转换成.vhdl格式的verilog程序，生成的.vhdl文件用于RTL级设计；对转换生成的VHDL的程序代码和RTL设计代码以及仿真文件在Vivado软件中进行仿真实验；经过仿真实验验证了代码的准确性,进而对verilog程序代码进行综合编译,布局布线,生成相应的硬件下载配置文件,将配置文件下载到FPGA器件上完成硬件验证。
上述反余弦计算模块由输入正负判断、反余弦查找表、线性插值、输出量化四部分组成，通过输入判断模块来确定输入的正负；将输入值进行缩放，使其值在反余弦查找表的地址范围内，并且通过查表获得相邻两个地址对应的反余弦值；经过线性插值模块进行拟合获得对应输入的反余弦值；通过输出量化模块并结合正负信息，得到最终的反余弦值。
有益效果是：
本发明提出了一种基于FPGA的机器人模型算法实现方法，提高了机器人模型算法的处理速度，该算法模块应用于主从控制机器人领域，在满足高精度测量的同时，还能达到高速实时的需求，同时具有低功耗、小型化、高速等特点，可应用于实施高精度的机器人控制领域，尤其是主从控制领域。
附图说明
图1为基于FPGA的机器人模型算法实现方法总体框图；
图2为串并转换模块实现框图；
图3为机器人模型计算模块总体框图；
图4为反余弦计算模块实现框图；
图5为主手机器人正运动学计算模块实现框图；
图6为算法使能模块实现框图；
图7为FPGA计算结果与理想结果对比图；
图8为FPGA计算结果与理想结果误差图；
具体实施方式
下面结合附图和实施例对本发明进一步说明
一种基于FPGA的机器人模型算法实现方法，其实现平台是FPGA，采用硬件描述语言实现，模块输入为上层处理器下传的串行数据流，如图1所示，机器人模型算法模块包括DMA子模块、串并转换子模块、FIFO子模块、机器人模型计算模块、算法使能模块、并串转换模块。
首先通过FPGA内的DMA子模块获得上层处理器的主手操作臂各关节的角度信息的串行数据流，并将串行数据流传给FIFO模块，之后串并转换子模块对串行数据流转为并行数据流传入FIFO子模块，之后送入机器人模型计算模块计算从手操作臂各个关节的角度信息，将得到的计算结果送入FIFO进行缓存，同时主手操作臂角度信息的FIFO和从手操作臂角度信息的FIFO的full信号送入算法使能模块用来确定机器人模型计算模块使能时间，最后经过并串转换模块将并行数据转换为串行数据，并且通过DMA模块回传给上层处理器。
上述一种基于FPGA的机器人模型算法实现方法，其步骤如下：
1)通过FPGA内的DMA子模块获得上层处理器的主手操作臂各关节的角度信息的串行数据流，并将串行数据流传给FIFO模块；
2)经过FIFO子模块缓存后送入串并转换子模块；
3)串并转换子模块对步骤2)生成的串行数据流转为并行数据流传入FIFO子模块；
4)经过FIFO子模块缓存步骤3)的数据后送入机器人模型计算子模块；
5)机器人模型计算子模块利用System Generator工具建立，其实现的功能为实现主手操作臂正运动学和从手操作臂逆运动学，将计算得到的从手操作臂的关节角度信息送入FIFO子模块；
6)经过FIFO子模块缓存步骤5)的数据后送入并串转化子模块；
7)步骤4)和步骤6)的FIFO的full信号送入算法使能模块；
8)算法使能模块用来控制算法计算模块的使能；
9)并串转换模块对步骤6)的并行数据进行处理得到串行数据流，并传入DMA模块；
10)通过DMA模块将步骤9)的数据回传给上层处理器；
上述DMA模块用于实现上层处理器和FPGA内的大数据交互，能大幅度提高数据交互的速度，从而提高系统的实时性。
上述FIFO模块用于对数据缓存，对提高系统的性能有很大的作用，并且能防止数据的丢失。
上述机器人模型计算模块包括反余弦计算模块、延时子模块、基本四则运算模块，延时子模块采用以为寄存器实现，本级功能模块的计算结果同时输出到下一级功能模块。基本四则运算模块采用流水线设计方法，提高计算速度。反余弦计算模块包括输入判断模块、反余弦查找表模块、线性插值模块、输出量化模块。
上述反余弦计算模块由输入正负判断、反余弦查找表、线性插值、输出量化四部分组成，通过输入判断模块来确定输入的正负；将输入值进行缩放，使其值在反余弦查找表的地址范围内，并且通过查表获得相邻两个地址对应的反余弦值；经过线性插值模块进行拟合获得对应输入的反余弦值；通过输出量化模块并结合正负信息，得到最终的反余弦值。
实施例
上述一种基于FPGA的机器人模型算法实现方法，DMA模块用于实现上层处理器和FPGA内的大数据交互，DMA模块从上层处理器接收的为主手各个关节的角度信息theam_s，向上层处理器回传的为从手各个关节的角度信息theas_s，数据格式均为axi4-stream，为串行数据流；
得到主手各个关节的角度信息的串行数据后，进入串并转换模块得到并行数据,,…,，串并转换模块实现框图如图2所示，根据计数器判断为第几个关节的角度信息，将这些数据分别送入FIFO进行缓存，送入机器人模型计算模块；
机器人模型计算模块主要需要求解机器人正、逆运动学计算，机器人模型总体结构如图3所示，将获取到的主手操作臂各关节的旋转角度通过主手操作臂的机器人正运动学计算获得主手操作臂末端准确的位置信息，将此位置信息通过从手操作臂的机器人逆运动学计算得到从手操作臂各关节的旋转角度。在机器人运动学求解中会遇到非线性函数计算，以反余弦函数计算为例进行说明，如图4所示，反余弦计算主要包括判断输入正负、反余弦查找表、线性插值和输出量化四个部分。我们假设求的反余弦值，通过放大倍将其映射为y，再通过缩小4倍即右移两位并且向下取整得到ROM表的输入地址m，通过查表得到，同时得到m+1对应的角度。继而通过线性插值算法得到。以主手机器人正运动学实现为例，主手操作臂正运动学的实现框图如图5所示，其中clk代表时钟周期，1clk代表第一个时钟周期，图5给出在特定的时钟周期内执行的运算表达式。通过设置各个模块的延迟时间保证本级模块的输出到达下级模块输入的同时性，避免产生数据丢失。将计算得到的从手各个关节的角度信息送入FIFO进行缓存，然后送入并串转换模块；
并串转换模块负责将从手各个关节的角度信息转换为串行数据，转化为DMA传输的数据格式axi4-stream，并且通过DMA回传给上层处理器。
设计中为了控制机器人模型计算模块的使能，加入了算法使能模块，实现框图如图6所示，通过主手操作臂各个关节角度信息的FIFO和从手操作臂各个关节角度信息的FIFO的full信号来确定合适的使能时机。
在Vivado中对模型进行综合、实现之后得到FPGA的资源利用率如表1所示，其中查找表和寄存器分别占用了36.28％和10.03％，其他资源的利用率均在正常范围内,符合设计要求。本文从快速性和准确性两个方面验证机器人模型设计。
表1 FPGA资源利用率
资源 使用 可用 使用率
查找表 19301 53200 36.28％
寄存器 10067 106400 10.03％
存储器 17 140 12.14％
DSP 92 220 41.82％
采用FPGA的在线调试工具，设置FPGA的工作频率为50MHz，主手三个关节的角度的变化趋势设置为：范围，变化间隔，由此得到从手第一个关节的角度与理想计算结果的对比曲线如图7所示。实线表示理想计算结果，虚线表示FPGA计算结果。在FPGA中模型计算时间为2.02微秒，在相同工作频率下，比型号TMS320C6713B的DSP芯片计算速度提升两百四十百倍以上。
将FPGA计算结果向前平移2.02微秒并与理想计算结果相减得到从手各关节的误差曲线如图8所示。其中从手第一个关节的最大误差为0.2694，第二个关节的最大误差为0.2494，第三个关节的最大误差为0.2491，能够满足机械臂的控制精度。
以上所述为本发明的较佳实施例而已,本发明不应该局限于该实施例和附图所公开的内容。凡是不脱离本发明所公开的精神下完成的等效或修改,都落入本发明保护的范围。

Claims (5)

1.一种基于FPGA的机器人模型算法实现方法，其实现平台为FPGA，采用硬件描述语言实现，模块输入为上层处理器传来的串行数据流，其特征在于，机器人模型算法模块包括DMA子模块、串并转换子模块、FIFO子模块、机器人模型计算模块、算法使能模块、并串转换模块：
其步骤如下：
1)通过FPGA内的DMA子模块获得上层处理器的主手操作臂各关节的角度信息的串行数据流，并将串行数据流传给FIFO模块；
2)经过FIFO子模块缓存后送入串并转换子模块；
3)串并转换子模块对步骤2)生成的串行数据流转为并行数据流传入FIFO子模块；
4)经过FIFO子模块缓存步骤3)的数据后送入机器人模型计算子模块；
5)机器人模型计算子模块利用System Generator工具建立，其实现的功能为实现主手操作臂正运动学和从手操作臂逆运动学，将计算得到的从手操作臂的关节角度信息送入FIFO子模块；
6)经过FIFO子模块缓存步骤5)的数据后送入并串转化子模块；
7)步骤4)和步骤6)的FIFO的full信号送入算法使能模块；
8)算法使能模块用来控制算法计算模块的使能；
9)并串转换模块对步骤6)的并行数据进行处理得到串行数据流，并传入DMA模块；
10)DMA模块将步骤9)的数据回传给上层处理器。
2.根据权利要求1所述的一种基于FPGA的机器人模型算法实现方法，其特征在于，所述DMA模块用于实现上层处理器和FPGA内的大数据交互，能大幅度提高数据交互的速度，从而提高系统的实时性。
3.根据权利要求1所述的一种基于FPGA的机器人模型算法实现方法，其特征在于，所述FIFO模块用于对数据缓存，对提高系统的性能有很大的作用，并且能防止数据的丢失。
4.根据权利要求1所述的一种基于FPGA的机器人模型算法实现方法，其特征在于，所述机器人模型计算模块包括反余弦计算模块、延时子模块、基本四则运算模块，设计步骤包括：
1)在Matlab/Simulink软件环境中设计机器人模型计算模块的Simulink模型文件；
2)完成模型文件设计后，利用Simulink的图形仿真工具，对设计的模型文件进行仿真；
3)在模型文件的设计时添加System Generator模块，利用该模块，将会把Simulink中设计好的.mdl文件转换成.vhdl格式的verilog程序，生成的.vhdl文件用于RTL级设计；
4)对转换生成的VHDL的程序代码和RTL设计代码以及仿真文件在Vivado软件中进行仿真实验；
5)经过仿真实验验证了代码的准确性,进而对verilog程序代码进行综合编译,布局布线,生成相应的硬件下载配置文件,将配置文件下载到FPGA器件上完成硬件验证。
5.根据权利要求4所述的一种基于FPGA的机器人模型算法实现方法，其特征在于，反余弦计算模块包括输入判断模块、反余弦查找表模块、线性插值模块、输出量化模块，通过以下步骤实现：
1)通过输入判断模块来确定输入的正负；
2)将输入值进行缩放，使其值在反余弦查找表的地址范围内，并且通过查表获得相邻两个地址对应的反余弦值；
3)经过线性插值模块进行拟合获得对应输入的反余弦值；
4)通过输出量化模块并结合步骤1)中的正负信息，得到最终的反余弦值。
CN201710016758.8A 2017-01-10 2017-01-10 一种基于fpga的机器人模型算法实现方法 Active CN106873447B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710016758.8A CN106873447B (zh) 2017-01-10 2017-01-10 一种基于fpga的机器人模型算法实现方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710016758.8A CN106873447B (zh) 2017-01-10 2017-01-10 一种基于fpga的机器人模型算法实现方法

Publications (2)

Publication Number Publication Date
CN106873447A true CN106873447A (zh) 2017-06-20
CN106873447B CN106873447B (zh) 2019-02-01

Family

ID=59157377

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710016758.8A Active CN106873447B (zh) 2017-01-10 2017-01-10 一种基于fpga的机器人模型算法实现方法

Country Status (1)

Country Link
CN (1) CN106873447B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108469726A (zh) * 2018-03-19 2018-08-31 南开大学 一种基于EthernetPowerlink的变参数PID运动控制器实现方法
CN110091329A (zh) * 2019-04-30 2019-08-06 河海大学常州校区 一种双机械臂的协同运动控制方法
CN113021344A (zh) * 2021-03-19 2021-06-25 南开大学 一种主从异构型遥操作机器人工作空间映射方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6081570A (en) * 1997-09-02 2000-06-27 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Parallel integrated frame synchronizer chip
CN103885919A (zh) * 2014-03-20 2014-06-25 北京航空航天大学 一种多dsp和fpga并行处理系统及实现方法
CN103927219A (zh) * 2014-05-04 2014-07-16 南京大学 可重构专用处理器核的周期精确的仿真模型及其硬件架构
CN104866640A (zh) * 2014-02-26 2015-08-26 龙羽 一种全fifo电路设计方法及其通用验证平台

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6081570A (en) * 1997-09-02 2000-06-27 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Parallel integrated frame synchronizer chip
CN104866640A (zh) * 2014-02-26 2015-08-26 龙羽 一种全fifo电路设计方法及其通用验证平台
CN103885919A (zh) * 2014-03-20 2014-06-25 北京航空航天大学 一种多dsp和fpga并行处理系统及实现方法
CN103927219A (zh) * 2014-05-04 2014-07-16 南京大学 可重构专用处理器核的周期精确的仿真模型及其硬件架构

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108469726A (zh) * 2018-03-19 2018-08-31 南开大学 一种基于EthernetPowerlink的变参数PID运动控制器实现方法
CN110091329A (zh) * 2019-04-30 2019-08-06 河海大学常州校区 一种双机械臂的协同运动控制方法
CN113021344A (zh) * 2021-03-19 2021-06-25 南开大学 一种主从异构型遥操作机器人工作空间映射方法

Also Published As

Publication number Publication date
CN106873447B (zh) 2019-02-01

Similar Documents

Publication Publication Date Title
CN103793562B (zh) 基于fpga的有源配电网暂态实时仿真系统设计方法
CN102184148B (zh) 一种基于fpga的at96总线控制器ip核及其构建方法
Dubey Introduction to embedded system design using field programmable gate arrays
CN106873447A (zh) 一种基于fpga的机器人模型算法实现方法
CN201426109Y (zh) 一种pci总线型多轴脉冲式运动控制卡
CN103941650B (zh) 逻辑与运动集成控制器
CN103901822A (zh) 一种多轴联动运动控制系统
Kinsy et al. Time-predictable computer architecture for cyber-physical systems: Digital emulation of power electronics systems
CN102799412A (zh) 基于并行流水线设计的cordic加速器
CN203849591U (zh) 多轴联动运动控制系统
CN204946009U (zh) 电力电子仿真系统
CN104899385B (zh) 异构多核的SoC设计评估系统
CN208588917U (zh) 一种基于arm+dsp+fpga的工业机器人运动控制器
Chen et al. A 3-D CPU-FPGA-DRAM hybrid architecture for low-power computation
CN106844900B (zh) 电磁暂态仿真系统的搭设方法
Tiwari et al. Design and Implementation of 4: 1 Multiplexer for Reversible ALU using QCA
CN201993640U (zh) 一种基于fpga的at96总线控制器ip核
CN103744352A (zh) 一种基于fpga的三次b样条曲线的硬件插补器
CN206224475U (zh) 一种mmc实时仿真建模系统
CN102789446A (zh) 基于cordic算法的dds信号杂散抑制方法及系统
CN201689326U (zh) 高速实时联动控制电路及芯片
Lim et al. Design and Implementation of IEC62541-based Industry-Internet of Things Simulator for Meta-Factory
CN105740179A (zh) 并行数据采集系统
Zhou et al. A real-time co-simulation research based on VSC closed-loop control
CN201681288U (zh) 一种基于java的数控设备

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