樊旭，嚴(yán)浩，何頂新

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

Zynq軟硬件平臺(tái)的仿人機(jī)器人逆運(yùn)動(dòng)學(xué)求解系統(tǒng)

樊旭，嚴(yán)浩，何頂新

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

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

仿人機(jī)器人；逆運(yùn)動(dòng)學(xué)； 軟硬件協(xié)同設(shè)計(jì)；XC7Z020-CLG484

引 言

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

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

1 系統(tǒng)平臺(tái)

1.1 機(jī)器人平臺(tái)

圖1 仿人機(jī)器人

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

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

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

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

1.2 硬件平臺(tái)

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

1.3 整體設(shè)計(jì)

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

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

2 機(jī)械臂逆運(yùn)動(dòng)學(xué)求解原理

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

圖4 機(jī)器人旋轉(zhuǎn)軸坐標(biāo)系

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

其中：

2.2 逆運(yùn)動(dòng)學(xué)求解

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

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

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

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

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

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

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

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

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

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

3.1 ARM部分設(shè)計(jì)

3.2 FPGA部分設(shè)計(jì)

3.2.1 逆運(yùn)動(dòng)學(xué)求解C語(yǔ)言

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

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

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

3.2.3 AXI接口設(shè)計(jì)

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

4 實(shí)驗(yàn)分析

4.1 誤差分析

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

表2 角度計(jì)算誤差

表3 FPGA資源使用

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

4.2 性能分析

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

表4 時(shí)間消耗對(duì)比

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

結(jié) 語(yǔ)

[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ī)械臂逆運(yùn)動(dòng)學(xué)解算與驗(yàn)證[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),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.

樊旭(碩士研究生)、何頂新(副教授)，研究方向?yàn)榍度胧较到y(tǒng)、智能機(jī)器人；嚴(yán)浩(碩士研究生)，研究方向是智能機(jī)器人、視頻編碼。

圖5 2個(gè)模塊訪問(wèn)不同bank的不同頁(yè)

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

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

結(jié) 語(yǔ)

參考文獻(xiàn)

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

[2] 馮超,張濤.TMS320C6678高速串行接口的傳輸性能研究[J].單片機(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)