一种基于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)机器人模型计算子模块利用SystemGenerator工具建立，其实现的功能为实现主手操作臂正运动学和从手操作臂逆运动学，将计算得到的从手操作臂的关节角度信息送入FIFO子模块；6)经过FIFO子模块缓存步骤5)的数据后送入并串转化子模块；7)步骤4)和步骤6)的FIFO的full信号送入算法使能模块；8)算法使能模块用来控制算法计算模块的使能；9)并串转换模块对步骤6)的并行数据进行处理得到串行数据流，并传入DMA模块；10)通过DMA模块将步骤9)的数据回传给上层处理器；上述DMA模块用于实现上层处理器和FPGA内的大数据交互，能大幅度提高数据交互的速度，从而提高系统的实时性。上述FIFO模块用于对数据缓存，对提高系统的性能有很大的作用，并且能防止数据的丢失。上述机器人模型计算模块包括反余弦计算模块、延时子模块、基本四则运算模块，设计步骤包括：在Matlab/Simulink软件环境中设计机器人模型计算模块的Simulink模型文件；完成模型文件设计后，利用Simulink的图形仿真工具，对设计的模型文件进行仿真；在模型文件的设计时添加SystemGenerator模块，利用该模块，将会把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)机器人模型计算子模块利用SystemGenerator工具建立，其实现的功能为实现主手操作臂正运动学和从手操作臂逆运动学，将计算得到的从手操作臂的关节角度信息送入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％，其他资源的利用率均在正常范围内,符合设计要求。本文从快速性和准确性两个方面验证机器人模型设计。表1FPGA资源利用率资源使用可用使用率查找表193015320036.28％寄存器1006710640010.03％存储器1714012.14％DSP9222041.82％采用FPGA的在线调试工具，设置FPGA的工作频率为50MHz，主手三个关节的角度的变化趋势设置为：范围，变化间隔，由此得到从手第一个关节的角度与理想计算结果的对比曲线如图7所示。实线表示理想计算结果，虚线表示FPGA计算结果。在FPGA中模型计算时间为2.02微秒，在相同工作频率下，比型号TMS320C6713B的DSP芯片计算速度提升两百四十百倍以上。将FPGA计算结果向前平移2.02微秒并与理想计算结果相减得到从手各关节的误差曲线如图8所示。其中从手第一个关节的最大误差为0.2694，第二个关节的最大误差为0.2494，第三个关节的最大误差为0.2491，能够满足机械臂的控制精度。以上所述为本发明的较佳实施例而已,本发明不应该局限于该实施例和附图所公开的内容。凡是不脱离本发明所公开的精神下完成的等效或修改,都落入本发明保护的范围。当前第1页1 2 3