芯耀
300
在机器人技术日新月异的今天，如何让机械臂更灵活、更精准地完成复杂任务，一直是科研与工业界关注的核心问题。尤其当机器人从平面走向立体，自由度从3个增加到6个时，运动规划的复杂度和计算量也成倍增长——传统的流水线式计算架构已难以满足实时求解的需求。
一种基于FPGA的创新设计正为此带来突破。通过构建一种高度精简的“最小计算单元”，并在此基础上实现矩阵运算的并行加速，研究者成功解决了6-DOF冗余度机械臂的运动规划问题。这种方案不仅大幅提升了计算效率，还能在有限硬件资源下实现末端轨迹的高精度跟踪，误差可控制在10⁻³至10⁻²量级。
本文将深入解读这一技术路径，从最小单元的结构设计，到整体坐标转换矩阵的并行计算，再到实际在PUMA560机械臂上的验证效果，一步步揭示FPGA如何赋能机器人实现更智能、更敏捷的控制。
冗余度机械臂的运动规划问题可以描述如下：
上述运动规划问题中的两个方程都是欠定的，拥有无穷多个可行解，这也给机器人的最优控制提供了可行解空间，使其在满足完成主要规划任务的前提下满足一些次级指标的性能优化。本节所实现的冗余度机器人是工业上的固定机械臂，其运动学方程中的Ｇ（θ） 是由机器人的结构决定的， 可以描述为一系列旋转矩阵的乘积。 在利用任意自由度的空间机器人执行末端跟踪任务时， 一般仅考虑机器人系统的末端在工作空间中的位置信息， 其运动学方程中的 Ｇ（θ）可以写为
其中，Ti为第ｉ个关节运动的空间坐标转换矩阵，即从第ｉ个关节坐标系到第ｉ－１个关节坐标系的坐标信息转换，Pe=[xen,yen,zen,1]T是由机器人末端到第ｎ个关节坐标系的坐标信息转换矢量。需要指出的是，θT1（θ1 ） 表示从第一个关节坐标系到世界基底坐标系之间的坐标信息转换。那么， 第 ｉ 个关节的坐标转换矩阵 Ti(i=1,…,n)可以描述为
其中，ｎ表示连杆的数量，θi表示第i个关节的旋转角度量， αi表示第i个坐标系和第i-1个坐标系X轴之间的角度，di表示第i个坐标系和第i-1个坐标系Z轴之间的距离，ai表示第i个坐标系和第i-1坐标系X轴之间的距离。 假设用经典DH参数定义法描述n-DOF的空间冗余度机器人的一般性结构， 见表1，θi、αi、di和ai被称为机器人的DH参数。表1 机器人的DH参数
因此，运动学方程中描述的雅可比矩阵为
本文实现的最小单元PE设计由1个乘法器、1个加法器、3个寄存以及5个输入／输出接口构成，如图1所示。两个输入接口分别为in_a和in_b，三个输出接口分别为out_a、out_b、out_c。它们的关系可以用out_c<=out_c+in_a×in_b，out_a<=in_a和out_b<=in_b表示， 其中符号 “＜＝” 表示寄存器打拍， 即在输入有效后， 一次打拍即可得到输出响应。
图1 最小单元PE设计示意图
基于上面提出的最小单元，进一步设计了坐标转换矩阵相乘单元。最小单元PE的设计结构，主要作用是将输入乘加并寄存，同时将输入延迟一拍传递给输出。本节使用了16个PE单元构成坐标转换矩阵相乘单元PEs，用来实现两个坐标转换矩阵的相乘运算，即TiTi-1。为了便于表述，将两个随机矩阵赋值给Ti和Ti-1，构成两个新的矩阵T'i和T'i-1 ：
图2描述了T'i和T'i-1
这两个矩阵的相乘， 其中相同颜色色块的PE表示同时被启动， 没有数字填充的PE表示未启动， 透明色块但有数字填充的PE表示计算完毕。 从图中可以看出， 每个PE并不是同时被启动的， 每个PE仅工作了三个周期就得到了结果，而整个PE需要10个周期就可以完成４×４的矩阵乘法运算。 此外， 为了保证数据输出结果的可用性， 在PE之间添加了 8个多路选择器MUX（见图3）， 可以保证输出数据的时序直接用于下一个PEs的输入。
图2 PE单元工作图
图3 基于多路选择器MUX的PE模块
基于上面提出的坐标转换矩阵相乘单元PEs，得到运动学方程的位置函数Ｇ（θ）以及雅可比矩阵 ＪＭ（θ）。 通过观察运动学方程中的 Ｇ（θ）， 发现其是由若干个坐标旋转矩阵Ti相乘， 最后右乘末端坐标信息转换向量Pe。 设计思路是从头至尾地选取两个旋转矩阵两两相乘， 再将其结果输入一个新的PEs中， 最后与PE相乘， 构成一个类似乘法树的结构， 如图4所示。图4表示冗余度机器人连杆数为6时，运动学方程中Ｇ（θ）函数的实现结构， 其中包括5个坐标转换矩阵相乘单元PEs、3个PE、12个坐标旋转生成模块以及若干坐标旋转生成模块与PEs之间的组合逻辑。坐标旋转生成模块是基于坐标旋转矩阵原理，生成矩阵Ti中的元素构成cosθｉ、sinθｉ、cosαｉ 以及sinαｉ， 其与PEs之间的组合逻辑用于将cosθｉ、sinθｉ、cosαｉ 以及sinαｉ 与 ai、di结合， 构成矩阵Ti中的元素， 并通过多路选择器按照规定好的时序输入给PEs。图2中有三个不同的色块，表明PEs按照三个不同时序分别完成。换句话说，该乘法树包含三个层次，第一步计算T1T2、T3T4和T5T6，第二步计算T1T2T3T4，第三步计算出T1T2T3T4T5T6。最后将得到的输出输入PE阵列，实现与PE相乘，即完成了运动学方程中Ｇ（θ）的计算。进一步地，也可以复用这个结构去实现运动学方程中的雅可比矩阵JM（θ）。
图4 整体设计架构图
基于前文描述的结构实现了PUMA560冗余度机械臂运动规划问题求解的任务，PUMA560的改进型DH参数已被列在表2中。将参数以查找表的形式存入Input块中，同时设置相应的实验环境与参数，然后将整个实现结构封装成一个协处理器。表2 PUMA560的改进型DH参数
机械臂的初始状态为θ（０）＝ ［1.675；2.843；-3.216；4.187；-1.71；-2.29］rad。 惩罚项系数设定为4。 其他参数初始设定为 γ ＝128、 σ ＝256以及 ｐ ＝0.0125。 解算过程的总时间被设定为 T=5s。 如图 5所示， 期待的路径和实际的末端执行器的轨迹显示中， 黑色虚线为期待的末端执行器路径， 蓝色点为实际的末端执行器的轨迹， 可以看出所实现的协处理器很好地执行了冗余度机械臂执行末端轨迹跟踪圆形轨迹的任务。所实现的协处理器的资源占用记录在表3中。位置误差如图5所示，其值可以达到10-3～10-2的微小范围，其中ex、ey和ez分别表示末端执行器的位置误差。
图5 冗余度机械臂执行末端效果图
表3 协处理器的资源占用记录
图6 冗余度机械臂执行末端位置误差
“十四五”时期国家重点出版物出版专项规划项目全面涵盖前沿趋势、技术原理与实战案例帮助读者掌握AI芯片的设计能力与前沿视野！
▊《人工智能芯片》张智军等
本书以图文并茂的方式系统性介绍了人工智能芯片的相关知识。主要内容如下：第1章探讨了人工智能芯片的概念、发展历史与分类。第2~5章分别描述了逻辑设计基础、HDL电路描述技巧、状态机设计与实现，以及数字设计方法与工程技巧。第6章详细讲述了不同类型传输协议的设计及具体实现方法。第7章深入探讨了存储结构的设计与实现。第8章则说明了特定的神经网络如何实现。第9章通过逻辑系统实际设计案例，帮助读者在实践中提升设计能力。
撰 稿 人：杨健亭责任编辑： 李馨馨审 核 人：曹新宇
