樊旭，嚴浩，何頂新

(華中科技大學(xué) 自動化學(xué)院，武漢 430074)

Zynq軟硬件平臺的仿人機器人逆運動學(xué)求解系統(tǒng)

樊旭，嚴浩，何頂新

(華中科技大學(xué) 自動化學(xué)院，武漢 430074)

仿人機器人的手臂模仿人體手臂設(shè)計，靈活性很好，能勝任更加復(fù)雜的工作，但是其機械結(jié)構(gòu)與傳統(tǒng)工業(yè)機器人有較大區(qū)別，進行逆運動學(xué)求解時會遇到復(fù)雜的非線性方程組求解問題，這導(dǎo)致不管是用解析法還是迭代法都會因為復(fù)雜的運算而導(dǎo)致速度下降，精度降低，不能滿足嵌入式系統(tǒng)的控制要求。機器人的幾何結(jié)構(gòu)可以提供更加直接的信息，幾何法與解析法的結(jié)合成為解決這一問題的有效方法之一。而在嵌入式平臺中，采用軟硬件協(xié)同設(shè)計則能夠進一步提高控制實時性以及系統(tǒng)可擴展性。

仿人機器人；逆運動學(xué)； 軟硬件協(xié)同設(shè)計；XC7Z020-CLG484

引 言

隨著機器人技術(shù)和人工智能的發(fā)展，機器人的應(yīng)用也越來越廣泛。機器人的逆運動學(xué)求解問題是機器人研究的基礎(chǔ)課題之一，也是一項具有挑戰(zhàn)性的工作。

目前基本上所有的機器人控制方案都是采用DSP[7]、微控制器[8]或者FPGA[9]實現(xiàn)，是一種純軟件或硬件解決方案。軟件方案有很好的擴展性，編程開發(fā)方便，但是系統(tǒng)性能不高，效率低，尤其是在機器人雙機械臂協(xié)同控制中問題尤為突出；硬件方案有很強性能，但是其開發(fā)語言HDL/Verilog難度較大，而且人機交互不易實現(xiàn)。下一代的機器人控制方案不僅需要更強的性能和更好的擴展性，而且對能耗與資源提出了更高的要求。這對現(xiàn)有方案提出了挑戰(zhàn)，本文在結(jié)合軟件和硬件實現(xiàn)的優(yōu)缺點情況下，提出采用軟硬件協(xié)同設(shè)計的方案，通過對功能的劃分，將任務(wù)合理分配給軟件和硬件來實現(xiàn)。另外針對FPGA開發(fā)難度大的問題，通過高層次綜合[10]的方法，將高級語言(例如C語言)直接轉(zhuǎn)成硬件描述語言，大大降低了系統(tǒng)開發(fā)難度。

1 系統(tǒng)平臺

1.1 機器人平臺

圖1 仿人機器人

機器人全身共有 22個關(guān)節(jié)，手臂尺寸完全按照正常人的比例設(shè)計，機器人的機械動作運行流暢順滑。大小臂如人體手臂一樣自由轉(zhuǎn)動、擺動，手腕可實現(xiàn)擺動和翻轉(zhuǎn)功能，手指如人手一般靈活，左右手可以相互配合完成不同動作。本文以機器人左臂為例研究逆運動學(xué)求解問題，機器人和尺寸如圖1和圖2所示，單臂屬于6個自由度機器人。

圖2 左臂結(jié)構(gòu)尺寸圖

機械人的逆運動學(xué)求解是為了研究各個坐標系之間的運動學(xué)關(guān)系，一般采用D-H參數(shù)法[11]建立機器人關(guān)節(jié)坐標系之間的變換關(guān)系。機器人左臂D-H參數(shù)如表1所列。

表1 機器人左臂D-H參數(shù)表

1.2 硬件平臺

Zynq平臺[12](XC7Z020-CLG484)包括處理器系統(tǒng)(PS)和可編程邏輯(PL)兩部分，其中處理器系統(tǒng)包含了Cortex-A9雙核ARM、輸入/輸出接口、內(nèi)部總線,以及存儲器接口等單元，可編程邏輯部分為Artix-7 FPGA。PS和PL既能相互獨立運行，也能聯(lián)合運行。PS部分資源十分完善，完全可以將其當作一個嵌入式系統(tǒng)開發(fā)平臺來使用，PL可以完成FPGA算法開發(fā)，通過與ARM互聯(lián)則可以實現(xiàn)算法加速與大數(shù)據(jù)處理等工作。

1.3 整體設(shè)計

系統(tǒng)采用軟硬件協(xié)同處理方法，充分發(fā)揮處理器與可編程邏輯的優(yōu)勢，ARM處理器擅長人機交互、流程控制及網(wǎng)絡(luò)等外設(shè)，F(xiàn)PGA擅長并行處理、算法加速。因此，機器人的交互控制命令由ARM處理，逆運動學(xué)算法復(fù)雜，交由FPGA處理，整體設(shè)計框圖如圖3所示。

圖3 整體設(shè)計框圖

2 機械臂逆運動學(xué)求解原理

2.1 機器人數(shù)學(xué)模型

圖4 機器人旋轉(zhuǎn)軸坐標系

相鄰旋轉(zhuǎn)軸的坐標系變換矩陣為：

其中：

2.2 逆運動學(xué)求解

機器人逆運動學(xué)求解實際上是求解旋轉(zhuǎn)軸的旋轉(zhuǎn)角度θ1、θ2、θ3、θ4、θ5、θ6的值。首先確定腕關(guān)節(jié)W點位置，根據(jù)幾何關(guān)系有：

基坐標系{x1,y1,z1}與手腕坐標系{x6,y6,z6}的關(guān)系為：

肩關(guān)節(jié)S點位于坐標系{x0,y0,z0}中的坐標為：(Sx,Sy,Sz)T=(145,0,0)T，位于坐標系{x6,y6,z6}中的坐標為：

求得肘關(guān)節(jié)E點位于坐標系{x0，y0,z0}中的坐標為：

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

坐標系{x0,y0,z0}與坐標系{x1,y1,z1}的變換關(guān)系為：

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

由機械臂小臂與手部的幾何關(guān)系知：

從求解過程可以知道，θ1、θ2、θ3、θ4、θ5、θ6實際上都有兩個解，但是各個角度受機器人結(jié)構(gòu)限制，大多數(shù)情況下只有一個解，在滿足機械結(jié)構(gòu)的情況下可以有多解。

3 系統(tǒng)設(shè)計

3.1 ARM部分設(shè)計

3.2 FPGA部分設(shè)計

3.2.1 逆運動學(xué)求解C語言

本文采用高層次綜合方法，算法采用軟件方法實現(xiàn)，并由 Vivado HLS工具轉(zhuǎn)成硬件描述語言Verilog，該工具支持將三角函數(shù)運算直接轉(zhuǎn)成硬件描述語言，但是不支持反三角函數(shù)運算，因此采用C語言實現(xiàn)時，三角函數(shù)可以采用解析式求解以提高精度，反三角函數(shù)則采用ROM查找表方法實現(xiàn)。ROM查找表需要構(gòu)建對應(yīng)的數(shù)組，正余弦函數(shù)的值域為[-1,1]，本文將梯度值設(shè)置為0.000 5，數(shù)組長度為4 000，用Matlab求解出對應(yīng)角度作為查找表的兩個數(shù)組，并以此為基礎(chǔ)進行查表運算。

3.2.2 設(shè)計優(yōu)化

除了對算法進行改善外，程序優(yōu)化的另一方面在于充分利用FPGA資源以降低時間消耗，因為硬件實現(xiàn)要求對FPGA資源使用率越高越好。本文在滿足要求的情況下，盡量用資源換取時間。Vivado HLS 工具提供了一系列優(yōu)化方式[13]，如在矩陣運算中有許多循環(huán)，可以采用循環(huán)展開的方式提升計算速度，數(shù)組存儲提供ROM_1P、ROM_2P、RAM_1P、RAM_2P等存儲方式，選擇不同的存儲方式也能改變執(zhí)行效率。

3.2.3 AXI接口設(shè)計

在異構(gòu)多核SoC中，核間高速通信非常重要，而擁有內(nèi)部AXI高速總線是Zynq平臺的優(yōu)勢。該總線在經(jīng)過內(nèi)部互聯(lián)后，可以支持多種通信機制，具備突發(fā)模式數(shù)據(jù)傳輸、DMA數(shù)據(jù)傳輸?shù)裙δ?。Zynq平臺的AXI總線包括AXI_HP、AXI_ACP、AXI_GP三種。

4 實驗分析

4.1 誤差分析

系統(tǒng)計算誤差主要來源于反三角函數(shù)運算中ROM 查找表的精度限制，因為ROM 查找表是離散定長的，精度有限。對于反正弦函數(shù)來說，在接近±1的時候誤差最大，而對于反余弦函數(shù)來說，在接近0的時候誤差最大。一般來說，查找表數(shù)組越長，精度越高。本文將查找表長度設(shè)置為4 000，通過實驗驗證得到了較好的效果，滿足精度和資源消耗的設(shè)計要求。計算誤差和資源消耗如表2和表3所列。

表2 角度計算誤差

表3 FPGA資源使用

通過表3可以看出，由FPGA計算的誤差不超過0.004rad，這對于機器人控制來說是完全滿足要求的。從表4中可以看出，DSP和LUT利用率都超過了50%，而BRAM資源達到38%。實際上，通過增加查找表數(shù)組長度可以提升BRAM的利用率，但是進一步增加查找表長度對精度的提升已經(jīng)很小，效果不大。該算法對寄存器資源需求不高，而FPGA提供的寄存器資源比較豐富，而利用率難以提升。整體而言，系統(tǒng)對FPGA的資源利用達到了較高的水平。

4.2 性能分析

系統(tǒng)性能主要考慮算法執(zhí)行效率，本文測試了同樣的逆運動學(xué)求解算法在嵌入式平臺上的執(zhí)行效率，并與Zynq平臺進行了對比，結(jié)果如表4所列。

表4 時間消耗對比

可以看出，本文提出的方案相較于傳統(tǒng)解決方案，時間消耗降低了3個數(shù)量級以上，大大提升了計算速度，證實了該方案的優(yōu)越性。

結(jié) 語

[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] 李憲華,郭永存.模塊化六自由度機械臂逆運動學(xué)解算與驗證[J].農(nóng)業(yè)機械學(xué)報,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.

樊旭(碩士研究生)、何頂新(副教授)，研究方向為嵌入式系統(tǒng)、智能機器人；嚴浩(碩士研究生)，研究方向是智能機器人、視頻編碼。

圖5 2個模塊訪問不同bank的不同頁

3.4 SRIO傳輸數(shù)據(jù)到共享RAM

DSP的共享RAM具有較大的總線寬度，最多支持4個bank的同時讀寫，也就是說最多支持4個主模塊進行讀寫操作。當FPGA通過SRIO將數(shù)據(jù)寫入共享RAM時候，就相對不存在頁切換，效率最高。但要注意的是，一旦SRIO對共享RAM進行讀寫時候，其他主模塊就不能對相應(yīng)頁進行操作，否則仍然會存在總線仲裁，導(dǎo)致數(shù)據(jù)丟失。但一般共享RAM只是用于幾個核或者核與外設(shè)之間的數(shù)據(jù)交互，所有核的程序不會在共享RAM中運行。建議SRIO在訪問共享RAM時候，及時將狀態(tài)反饋到各個核，從而避免多個主模塊同時訪問同一空間。使用硬件信號量和核間中斷可以完成狀態(tài)的反饋。

結(jié) 語

參考文獻

[1] 許樹軍,黃镠,牛戴楠,等.基于FPGA的Serial RapidIO協(xié)議的設(shè)計與實現(xiàn)[J].雷達與對抗,2015,35(4):54-56.

[2] 馮超,張濤.TMS320C6678高速串行接口的傳輸性能研究[J].單片機與嵌入式系統(tǒng)應(yīng)用,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.

(責(zé)任編輯：薛士然 收稿日期：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)