引言

随着科技的进步，机器人技术正在向智能机器和智能系统的方向发展，其发展趋势主要为结构的模块化和可重构化；控制技术的开放化、可配置化；伺服驱动技术的数字化和分散化；多传感器融合技术的实用化。机器人的内涵也已变为“灵活应用机器人技术的、具有实在动作功能的智能化系统”。近十几年来，FPGA行业也在飞速的发展，它在实际科研开发中的地位也从处理简单逻辑上升到了数字系统的核心处理器件。SOPC(可编程片上系统)技术的出现，使得将CPU核与设备核以及系统软件集成到单一芯片中成为可行，它能够帮助用户快速开发出所需要的产品。

本文结合实际项目以一个固定基座的单臂六自由度机器人作为研究对象，提出一种基于FPGA的模块化机器人嵌入式多核主控制器设计方案。

1 机器人路径规划

设机械臂初始和终止位姿分别记为：Xe0=[Pe0，ψe0]，Xef=[Pef，ψef]。要求机械臂末端手爪沿Xe0到Xef的直线路径运动，起点和终点分别为Pe0(x0．y0，z0，α0，β0，γ0)和Pef(xf，yf，zf，αf，βf，γf)，机器人手抓运行轨迹如图1所示。





由初始点和终止点坐标计算出首末端直线距离长度为：



对于一个路径段，由于两端的过渡域具有相同的持续时间，因而在这两个域中，采用相同的恒加速度值，只有符号相反，带抛物线过渡的线性插值如图2所示。[page]



在求解加速度的基础上可以得出末端运行线速度，末端线速度分为加速段、匀速段和减速段，其具体表达式为：



在得到了线速度和角速度之后就可以通过雅克比矩阵得出各个关节的关节角速度，再乘以固定的时间即可得出在每一步的关节转角。

2 Nios II软核体系结构

Nios II是Altera公司的第二代用户可配置的通用32位RISC软核处理器，是Altera公司特有的基于通用FPGA架构的CPU软核。它具备完整的32位指令集、32位数据通道和地址空间；带有32个通用寄存器；支持32个外部中断源；单指令的32位与32位乘法和除法结果是32位；对于结果为64位或128位的乘法，提供专用指令；大多数指令可以在一个时钟周期内完成；带有单指令桶形移位寄存器；可以访问各种片上外设，提供与片外存储器和外设的接口；处理器性能超过200 DMIPS。

Nios II是一个可配置的软核处理器，用户可以根据性能和成本的要求来增加或删减处理器的功能。Nios II处理器不像ARM那样是由固定的芯片来实现，而是采用IP核的方式实现的，可以配置在满足任何要求的AlteraFPGA器件中，因此，Nios II处理器给实际应用带来了很大的灵活性。只要芯片上有足够的空间，就可以不断进行升级而不用修改电路结构。另外，Nios II处理器作为一个标准的RISC处理器，具有执行标准的C源代码和程序的可移植性强等特点。

3 协处理器结构

由于模块化机器人的控制需要进行大量复杂的三角函数运算以及矩阵运算，单一Nios II软核处理器在完成这些运算的同时并不能保证控制的快速性、实时性要求。因此本文设计一个专用的IP软核作为协处理器，用来进行各种机器人运动学的解算。



协处理器总体设计框架如图3所示。GDB、GAB、GCB分别表示外部的数据、地址和控制总线。协处理器主要包含输入／输出接口逻辑、机器人参数存储区、协处理器运算控制模块、运算处理模块、命令字寄存器、协处理器状态字寄存器和输入／输出FIFO构成。



在本设计中，协处理器作为一个I／O设备与主处理器之间连接，协处理器完全接受主处理器的调度和支配。由于机器人控制算法的调度与计算是一个周期性过程，因此采用I／O设备方式即可满足要求。采用此方式系统虽然增加了协处理器，但却没有增加复杂度，利于保证核心部件的可靠性且易于实现和管理，能够简化软件设计。针对该接口模式的设计，协处理器与主处理器之间的硬件接口结构框图如图4所示。

协处理器面向Nios II处理器接口分为参数区和寄存器区，通过写命令寄存器来识别。通常情况下，给出的CMD为正常计算模式，默认选择寄存器区；当需要对基本参数进行访问／修改时，给出的CMD为修改参数模式。其中寄存器区包括各类寄存器和输入输出FIFO。图4中各信号含义在表1内进行了介绍。

4 片上多核结构

利用SOPC技术把Nios II软核、专用协处理器、存储器控制IP、CAN总线控制逻辑、内部计数器等全部集成到FPGA芯片上。基于SOPC的多核控制器的系统结构如图5所示。[page]



整个分布式控制系统结构如图6所示，多核主控制器通过CAN总线与各关节控制器通信并对反馈信息进行处理，进行路径规划，下发关节角。各关节控制器采集电机当前角度信息并通过电机驱动模块控制电机运动。

5 实验及结果分析

实验系统包括基于FPGA的嵌入式多核主控制器、机器人关节控制器以及单臂六自由度机器人等。运动学正解运算性能时间对比如表2所列。在保证控制准确、有效的前提下分别采用单Nios II软核处理器与协处理器进行运动学正解运算。单Nios II软核处理的耗时约为2900

μs，而多核主控制器的协处理器只需要约10μs的时间。可以看出在相同系统时钟下，协处理器进行运动学正解比单Nios II软核处理器的效率提升了两个数量级。





表3列出了一个控制周期(多核主控制器采用路径规划算法控制机器人从空间中一点到下一点)内各阶段的处理时间，整个控制周期所需时间约为24．97 ms，比之前单独采用Nios II软核处理器控制的100ms节约了大概3／4的时间。而且，用于CAN总线通信及数据采集的部分(获取目标位姿与当前关节角、下发下一步关节角)所占用的时间为整个周期的76％。由此可以看出，该设计能极大地提高系统处理能力，并能够很好解决嵌入式控制器运算能力对模块化机器人控制系统的约束问题。

6 结论

本文对基于FPGA的模块化机器人嵌入式多核主控制器进行了研究，重点讨论了路径规划方法，分析了NiosII软核体系结构；并对协处理器结构设计、片上结构设计做了详细介绍。最终实验结果也表明了该设计的可行性以及控制的快速性。在更改机器人结构的时候，只需要分析连杆坐标系，获得D-H参数并传递给协处理器，该多核主控制器仍能可靠有效地工作。