樊旭，严浩，何顶新

(华中科技大学 自动化学院，武汉 430074)

Zynq软硬件平台的仿人机器人逆运动学求解系统

樊旭，严浩，何顶新

(华中科技大学 自动化学院，武汉 430074)

仿人机器人的手臂模仿人体手臂设计，灵活性很好，能胜任更加复杂的工作，但是其机械结构与传统工业机器人有较大区别，进行逆运动学求解时会遇到复杂的非线性方程组求解问题，这导致不管是用解析法还是迭代法都会因为复杂的运算而导致速度下降，精度降低，不能满足嵌入式系统的控制要求。机器人的几何结构可以提供更加直接的信息，几何法与解析法的结合成为解决这一问题的有效方法之一。而在嵌入式平台中，采用软硬件协同设计则能够进一步提高控制实时性以及系统可扩展性。

仿人机器人；逆运动学； 软硬件协同设计；XC7Z020-CLG484

引 言

随着机器人技术和人工智能的发展，机器人的应用也越来越广泛。机器人的逆运动学求解问题是机器人研究的基础课题之一，也是一项具有挑战性的工作。

目前基本上所有的机器人控制方案都是采用DSP[7]、微控制器[8]或者FPGA[9]实现，是一种纯软件或硬件解决方案。软件方案有很好的扩展性，编程开发方便，但是系统性能不高，效率低，尤其是在机器人双机械臂协同控制中问题尤为突出；硬件方案有很强性能，但是其开发语言HDL/Verilog难度较大，而且人机交互不易实现。下一代的机器人控制方案不仅需要更强的性能和更好的扩展性，而且对能耗与资源提出了更高的要求。这对现有方案提出了挑战，本文在结合软件和硬件实现的优缺点情况下，提出采用软硬件协同设计的方案，通过对功能的划分，将任务合理分配给软件和硬件来实现。另外针对FPGA开发难度大的问题，通过高层次综合[10]的方法，将高级语言(例如C语言)直接转成硬件描述语言，大大降低了系统开发难度。

1 系统平台

1.1 机器人平台

图1 仿人机器人

机器人全身共有 22个关节，手臂尺寸完全按照正常人的比例设计，机器人的机械动作运行流畅顺滑。大小臂如人体手臂一样自由转动、摆动，手腕可实现摆动和翻转功能，手指如人手一般灵活，左右手可以相互配合完成不同动作。本文以机器人左臂为例研究逆运动学求解问题，机器人和尺寸如图1和图2所示，单臂属于6个自由度机器人。

图2 左臂结构尺寸图

机械人的逆运动学求解是为了研究各个坐标系之间的运动学关系，一般采用D-H参数法[11]建立机器人关节坐标系之间的变换关系。机器人左臂D-H参数如表1所列。

表1 机器人左臂D-H参数表

1.2 硬件平台

Zynq平台[12](XC7Z020-CLG484)包括处理器系统(PS)和可编程逻辑(PL)两部分，其中处理器系统包含了Cortex-A9双核ARM、输入/输出接口、内部总线,以及存储器接口等单元，可编程逻辑部分为Artix-7 FPGA。PS和PL既能相互独立运行，也能联合运行。PS部分资源十分完善，完全可以将其当作一个嵌入式系统开发平台来使用，PL可以完成FPGA算法开发，通过与ARM互联则可以实现算法加速与大数据处理等工作。

1.3 整体设计

系统采用软硬件协同处理方法，充分发挥处理器与可编程逻辑的优势，ARM处理器擅长人机交互、流程控制及网络等外设，FPGA擅长并行处理、算法加速。因此，机器人的交互控制命令由ARM处理，逆运动学算法复杂，交由FPGA处理，整体设计框图如图3所示。

图3 整体设计框图

2 机械臂逆运动学求解原理

2.1 机器人数学模型

图4 机器人旋转轴坐标系

相邻旋转轴的坐标系变换矩阵为：

其中：

2.2 逆运动学求解

机器人逆运动学求解实际上是求解旋转轴的旋转角度θ1、θ2、θ3、θ4、θ5、θ6的值。首先确定腕关节W点位置，根据几何关系有：

基坐标系{x1,y1,z1}与手腕坐标系{x6,y6,z6}的关系为：

肩关节S点位于坐标系{x0,y0,z0}中的坐标为：(Sx,Sy,Sz)T=(145,0,0)T，位于坐标系{x6,y6,z6}中的坐标为：

求得肘关节E点位于坐标系{x0，y0,z0}中的坐标为：

(Ex,Ey,Ez)T=R06(HEx,HEy,HEz)T+(Wx,Wy,Wz)T

坐标系{x0,y0,z0}与坐标系{x1,y1,z1}的变换关系为：

θ2=cos-1(0.9660·Δx1-0.2588·Δz1)

由机械臂小臂与手部的几何关系知：

从求解过程可以知道，θ1、θ2、θ3、θ4、θ5、θ6实际上都有两个解，但是各个角度受机器人结构限制，大多数情况下只有一个解，在满足机械结构的情况下可以有多解。

3 系统设计

3.1 ARM部分设计

3.2 FPGA部分设计

3.2.1 逆运动学求解C语言

本文采用高层次综合方法，算法采用软件方法实现，并由 Vivado HLS工具转成硬件描述语言Verilog，该工具支持将三角函数运算直接转成硬件描述语言，但是不支持反三角函数运算，因此采用C语言实现时，三角函数可以采用解析式求解以提高精度，反三角函数则采用ROM查找表方法实现。ROM查找表需要构建对应的数组，正余弦函数的值域为[-1,1]，本文将梯度值设置为0.000 5，数组长度为4 000，用Matlab求解出对应角度作为查找表的两个数组，并以此为基础进行查表运算。

3.2.2 设计优化

除了对算法进行改善外，程序优化的另一方面在于充分利用FPGA资源以降低时间消耗，因为硬件实现要求对FPGA资源使用率越高越好。本文在满足要求的情况下，尽量用资源换取时间。Vivado HLS 工具提供了一系列优化方式[13]，如在矩阵运算中有许多循环，可以采用循环展开的方式提升计算速度，数组存储提供ROM_1P、ROM_2P、RAM_1P、RAM_2P等存储方式，选择不同的存储方式也能改变执行效率。

3.2.3 AXI接口设计

在异构多核SoC中，核间高速通信非常重要，而拥有内部AXI高速总线是Zynq平台的优势。该总线在经过内部互联后，可以支持多种通信机制，具备突发模式数据传输、DMA数据传输等功能。Zynq平台的AXI总线包括AXI_HP、AXI_ACP、AXI_GP三种。

4 实验分析

4.1 误差分析

系统计算误差主要来源于反三角函数运算中ROM 查找表的精度限制，因为ROM 查找表是离散定长的，精度有限。对于反正弦函数来说，在接近±1的时候误差最大，而对于反余弦函数来说，在接近0的时候误差最大。一般来说，查找表数组越长，精度越高。本文将查找表长度设置为4 000，通过实验验证得到了较好的效果，满足精度和资源消耗的设计要求。计算误差和资源消耗如表2和表3所列。

表2 角度计算误差

表3 FPGA资源使用

通过表3可以看出，由FPGA计算的误差不超过0.004rad，这对于机器人控制来说是完全满足要求的。从表4中可以看出，DSP和LUT利用率都超过了50%，而BRAM资源达到38%。实际上，通过增加查找表数组长度可以提升BRAM的利用率，但是进一步增加查找表长度对精度的提升已经很小，效果不大。该算法对寄存器资源需求不高，而FPGA提供的寄存器资源比较丰富，而利用率难以提升。整体而言，系统对FPGA的资源利用达到了较高的水平。

4.2 性能分析

系统性能主要考虑算法执行效率，本文测试了同样的逆运动学求解算法在嵌入式平台上的执行效率，并与Zynq平台进行了对比，结果如表4所列。

表4 时间消耗对比

可以看出，本文提出的方案相较于传统解决方案，时间消耗降低了3个数量级以上，大大提升了计算速度，证实了该方案的优越性。

结 语

[1] Martin Pfurner.A new and efficient algorithm for the inverse kinematics of a general serial 6R manipulator[J].Mechanism and Machine Theory,2005,42(1):66-81.

[2] Zhongtao Fu,Wenyu Yang.Solution of Inverse Kinematics for 6R Robot Manipulators with Offset Wrist Based on Geometric Algebra[J].Journal of Mechanisms and Robotics,2013,5(3):310081-310087.

[3] 李宪华,郭永存.模块化六自由度机械臂逆运动学解算与验证[J].农业机械学报,2013,44(4):246-251.

[4] Huashan Liu,Wuneng Zhou.An Efficient Inverse Kinematic Algorithm for a PUMA560-Structured Robot Manipulator[J].International Journal of Advanced Robotic Systems,2013,10(3):1-5.

[5] Philippe Bidaud.Closed form solutions for inverse kinematics approximation of general 6R manipulators[J].Mechanism and Machine Theory,2004,39(3):323-338.

[6] Andreas Aristidou,Joan Lasenby.FABRIK:A fast, iterative solver for the Inverse Kinematics problem[J].Graphical Models,2011,73(5):243-260.

[7] Bo You,Dongjie Li,Suju Liu.Design of DSP-based Open Control System for Industrial Robot[C]//International Conference on Automation and Logistics,2007:1585-1590.

[8] Jegede Olawale,Awodele Oludele,Ajayi Ayodele.Development of a Microcontroller Based Robotic Arm[J].Diagnostic Pathology,2007,8(6):705-716.

[9] Ying-Shieh Kung,Gua-Shieh Shu,Development of a FPGA-based Motion Control IC for Robot Arm[C]//IEEE International Conference on Industrial Technology,2005.

[10] Wim Meeus,Kristof Van Beeck.An overview of today’s high-level synthesis tools[J].Design Automation for Embedded Systems,2012,16(3):31-51.

[11] Denavit J,Hartenberg R S.A kinematic notation for lower-pair mechanisms based on matrices[J].Journal of Applied Mechanics,1955,22(6):215-221.

[12] Xilinx.Zynq-7000 All Programmable SoC Technical Reference Manual [EB/OL].[2016-09].http://www.xilinx.com/support/documentation/user_guides/ug585-Zynq-7000-TRM. pdf.

[13] Xilinx.Vivado Design Suite User Guide High-Level Synthesis [EB/OL].[2016-09].http://www.xilinx.com/support/documentation/sw_manuals/xilinx2016_2/ug902-vivado-high-level-synthesis.pdf.

樊旭(硕士研究生)、何顶新(副教授)，研究方向为嵌入式系统、智能机器人；严浩(硕士研究生)，研究方向是智能机器人、视频编码。

图5 2个模块访问不同bank的不同页

3.4 SRIO传输数据到共享RAM

DSP的共享RAM具有较大的总线宽度，最多支持4个bank的同时读写，也就是说最多支持4个主模块进行读写操作。当FPGA通过SRIO将数据写入共享RAM时候，就相对不存在页切换，效率最高。但要注意的是，一旦SRIO对共享RAM进行读写时候，其他主模块就不能对相应页进行操作，否则仍然会存在总线仲裁，导致数据丢失。但一般共享RAM只是用于几个核或者核与外设之间的数据交互，所有核的程序不会在共享RAM中运行。建议SRIO在访问共享RAM时候，及时将状态反馈到各个核，从而避免多个主模块同时访问同一空间。使用硬件信号量和核间中断可以完成状态的反馈。

结 语

参考文献

[1] 许树军,黄镠,牛戴楠,等.基于FPGA的Serial RapidIO协议的设计与实现[J].雷达与对抗,2015,35(4):54-56.

[2] 冯超,张涛.TMS320C6678高速串行接口的传输性能研究[J].单片机与嵌入式系统应用,2015,11(4):52-55.

[3] Texas Instruments Inc.TMS320C6678 Multicore Fixed and Floating-Point Digital Signal Processor Data Manual,2013.

[4] Texas Instruments Inc.KeyStone Architecture Serial Rapid IO (SRIO) User Guide,2012.

(责任编辑：薛士然 收稿日期：2016-08-17)

Inverse Kinematics of Robot Based on Zynq Platform

Fan Xu,Yan Hao,He Dingxin

(School of Automation,Huazhong University of Science and Technology,Wuhan 430074,China)

The arm of humanoid robot is designed to mimic the human body,which gives it good flexibility and capable of more complex work.However,its mechanical structure is quite different from the industrial robot,solving nonlinear equations will be a problem when dealing with the inverse kinematics.It will reduce the computing speed and bring worse accuracy,and makes it more difficult to be applied in the embedded system.While its geometric properties may give more effective information,the combination of geometric and closed-form method provides a good way to solve the problem.Besides,the software and hardware co-design method can further improve the real-time control and scalability.

humanoid robot;inverse kinematic;software and hardware co-design;XC7Z020-CLG484

TP242

A

�士然

2016-09-09)