基于模型設(shè)計(jì)方法的機(jī)器人柔性運(yùn)動(dòng)控制算法的實(shí)現(xiàn)

唐志祺， 李 鈺， 包懌杰， 肖家麟

（華東理工大學(xué)信息科學(xué)與工程學(xué)院，上海 200237）

工業(yè)4.0 和中國制造2025 計(jì)劃推動(dòng)了國內(nèi)工業(yè)機(jī)器人產(chǎn)業(yè)的快速增長[1]。運(yùn)動(dòng)控制算法作為機(jī)器人控制器的核心技術(shù)，對(duì)機(jī)器人運(yùn)動(dòng)性能有重要影響。伺服系統(tǒng)的速度控制是運(yùn)動(dòng)控制器的重要工作，柔性控制的目的是實(shí)現(xiàn)速度的平穩(wěn)變化，增加系統(tǒng)的穩(wěn)定性，同時(shí)盡可能降低系統(tǒng)的效率損失[2]。在運(yùn)動(dòng)控制系統(tǒng)中，主要有直線（含梯形）、三角函數(shù)、指數(shù)、拋物線、S 形等加減速算法[3]，其中，S 形加減速算法因柔性最佳而受到廣泛關(guān)注，但是該算法中涉及三階多項(xiàng)式運(yùn)算，制約了其在低端嵌入式運(yùn)動(dòng)控制器中的應(yīng)用。

有許多國內(nèi)外學(xué)者為實(shí)現(xiàn)柔性控制算法應(yīng)用于嵌入式系統(tǒng)進(jìn)行了深入的研究。Huang 等[4]采用數(shù)字卷積技術(shù)通過在FPGA 中嵌入軟核處理器與硬件配合實(shí)現(xiàn)多種加減速控制算法，但該算法無法配置初始速度，并且應(yīng)用軟核對(duì)硬件資源損耗過大，不利于系統(tǒng)多軸擴(kuò)展。劉鵬等[5]采用FPGA 實(shí)現(xiàn)了梯形加減速控制，在此基礎(chǔ)上引入滑動(dòng)濾波器思想實(shí)現(xiàn)了等效的S 形加減速效果，解決了加速度跳變問題，但未考慮濾波器引起的延時(shí)對(duì)系統(tǒng)實(shí)時(shí)性和連續(xù)性造成的不良影響。文獻(xiàn)[6]采用查表法存儲(chǔ)按時(shí)間間隔的轉(zhuǎn)速值，實(shí)現(xiàn)過程簡單、快速，然而查表法受采樣密度影響導(dǎo)致容量受限，限制了S 形曲線隨參數(shù)變化而變化的靈活性。蔡錦達(dá)等[7]在Cortex-M3 開發(fā)板上實(shí)現(xiàn)加減速柔性控制，提出了通過指針函數(shù)和動(dòng)態(tài)數(shù)組進(jìn)行程序設(shè)計(jì)的方法，但是該設(shè)計(jì)思路與查表法如出一轍，而且內(nèi)核中定時(shí)器資源有限，不易功能擴(kuò)展。上述算法的實(shí)現(xiàn)通常以基于代碼的形式開發(fā)，并以實(shí)體形式進(jìn)行測試，開發(fā)過程單一且較容易出錯(cuò)[8]，尤其是通用性差，對(duì)象一旦變化就要推倒重來。

本文針對(duì)機(jī)器人柔性S 形加減速運(yùn)動(dòng)控制算法嵌入復(fù)雜的問題，提出了一種有效的軟硬件協(xié)同實(shí)現(xiàn)策略，旨在充分利用軟件和硬件的優(yōu)勢實(shí)現(xiàn)算法功能，同時(shí)兼顧系統(tǒng)開銷、時(shí)序等方面的要求。與此同時(shí)，將可視化的基于模型設(shè)計(jì)（Model-Based Design，MBD）方法引入到軟硬件協(xié)同設(shè)計(jì)中，進(jìn)一步提高算法通用性和開發(fā)效率?；谀Ｐ驮O(shè)計(jì)方法因其圖形化設(shè)計(jì)優(yōu)勢，在汽車、航空、能源等領(lǐng)域得到廣泛應(yīng)用[9-11]，國內(nèi)外許多大型項(xiàng)目中均采用這種高效的開發(fā)方法。Airbus 公司使用基于模型設(shè)計(jì)的方法對(duì)空客A380 的燃油系統(tǒng)進(jìn)行建模和控制模型設(shè)計(jì)，大大加速了各部門的協(xié)調(diào)開發(fā)，在沒有增加員工的情況下提前數(shù)月完成了燃油控制管理系統(tǒng)的設(shè)計(jì)[12]。汽車行業(yè)非常重視項(xiàng)目的需求驗(yàn)證，采用基于模型設(shè)計(jì)方法可為其最大程度地縮短開發(fā)效率，降低成本并提高生產(chǎn)率，因?yàn)榇蠖鄶?shù)代碼都是通過模型自動(dòng)生成的[13]。

基于模型設(shè)計(jì)方法在機(jī)器人柔性運(yùn)動(dòng)控制設(shè)計(jì)方面的應(yīng)用鮮見報(bào)道。本文引入這一先進(jìn)的設(shè)計(jì)思想，以機(jī)器人柔性S 形加減速運(yùn)動(dòng)控制算法為研究對(duì)象，將其應(yīng)用于基于SoC 的運(yùn)動(dòng)控制系統(tǒng)，進(jìn)一步拓展其在機(jī)器人運(yùn)動(dòng)控制領(lǐng)域中的應(yīng)用，為個(gè)性化機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的快速設(shè)計(jì)提供思路。首先對(duì)柔性S 形加減速算法建模，建立一組易于解算的接口參數(shù)列表，分析不同參數(shù)約束下其速度曲線的變化規(guī)律；在此基礎(chǔ)上，引入基于模型設(shè)計(jì)方法在Simulink 中為算法設(shè)計(jì)軟硬件模型并通過仿真測試，進(jìn)而利用MathWorks 工具箱為模型自動(dòng)生成代碼并將其部署到以Zynq-7000 為核心的運(yùn)動(dòng)控制器；最后，通過搭建實(shí)驗(yàn)平臺(tái)以驗(yàn)證基于模型設(shè)計(jì)方法的可行性與可靠性。

1 柔性運(yùn)動(dòng)控制算法建模與仿真

1.1 S 形加減速算法的運(yùn)動(dòng)學(xué)建模

S 形加減速算法因其曲線呈S 形而得名，其特點(diǎn)是柔性好。如圖1 所示，加速度曲線呈梯形形狀，速度曲線是光滑的，可以保證運(yùn)動(dòng)控制系統(tǒng)的平穩(wěn)性。運(yùn)動(dòng)曲線有3 個(gè)速度區(qū)（加速、恒速、減速），按運(yùn)行次序細(xì)分為7 段，依次為加加速段T1、勻加速段T2、減加速段T3、恒速段T4、加減速段T5、勻減速段T6、減減速段T7。其運(yùn)動(dòng)學(xué)模型表示為

圖1 S 形加減速運(yùn)動(dòng)曲線Fig. 1 Motion profiles of S-shaped ACC/DEC

tm的值越大，表示電機(jī)的柔性越大，速度曲線趨于平滑；反之，tm的值越小，則沖擊越大，速度曲線趨向梯形速度曲線，如圖2 所示。其中S?表示各段速度曲線下的面積，亦表示各階段位移，可由式(1)積分獲得。根據(jù)對(duì)稱性條件，有

圖2 柔性S 形速度曲線Fig. 2 Flexible S-shaped velocity profile

為突出速度曲線柔性特征，本文采用的接口參數(shù)與傳統(tǒng)參數(shù)有所區(qū)別，傳統(tǒng)參數(shù)通常為起始速度v0、最大允許速度vm、最大加速度am和加加速度Jm[3-4,7,15]，而本文將am和Jm替換為總加速時(shí)間tr與tm，這樣在建模過程中可有效避免三次多項(xiàng)式[15]的求解，從而為運(yùn)動(dòng)控制器減輕計(jì)算負(fù)擔(dān)。

在實(shí)際應(yīng)用中S 形加減速算法不可能完整地包含圖2 中的7 段曲線[16]。速度曲線將依據(jù)參數(shù)以及運(yùn)動(dòng)路徑L的設(shè)置相應(yīng)地發(fā)生變化，故S 形加減速算法不是固定的7 個(gè)階段。

在分析前，應(yīng)對(duì)已知參數(shù)實(shí)施約束。本文設(shè)v0、tr和tm為不變量，最大允許速度vm因運(yùn)動(dòng)路徑L的大小而變化。在此約定下，規(guī)劃后的S 形曲線主要存在以下幾種情形：

(1) 存在恒速段T4，即L>Lα。Lα為恒速段臨界位移，表示系統(tǒng)正好從初始速度運(yùn)行到達(dá)最大允許速度時(shí)的情況，定義為

①當(dāng)tm=0 時(shí)，S 形曲線退化為直線，為標(biāo)準(zhǔn)的梯形速度曲線。由式(1)、式（3）、式（4）獲得下列參數(shù)：

對(duì)不同條件下的柔性S 形加減速算法分類，分類結(jié)果如表1 所示。

表1 不同條件下的柔性S 形加減速算法Table 1 S-shaped ACC/DEC algorithms under different conditions

1.2 S 形加減速算法仿真

基于先前的分析，采用MATLAB 2020a 對(duì)不同條件下的S 形加減速算法進(jìn)行仿真。當(dāng)仿真數(shù)據(jù)為v0=100 mm/s ，vm=600 mm/s ，tr=0.6 s ，tm分別為0、0.15、0.3 s 時(shí)，分析不同運(yùn)動(dòng)路徑下S 形速度曲線的變化情況，仿真結(jié)果如圖3 所示。

圖3 不同條件下S 形加減速算法的仿真結(jié)果Fig. 3 Simulation results of S-shaped ACC/DEC algorithm under different conditions

當(dāng)L=800 mm 時(shí)，不同特征時(shí)間常數(shù)下的速度曲線均存在勻速段，體現(xiàn)為梯形速度曲線、7 段S 形速度曲線和5 段S 形速度曲線。

當(dāng)L=100 mm 時(shí)，計(jì)算可知加速段臨界位移Lβ=120 mm，因此S 形加減速算法表現(xiàn)為勻速運(yùn)動(dòng)。根據(jù)式(11)得到運(yùn)行時(shí)間tβ=1 s ，仿真結(jié)果與理論結(jié)果一致。

2 基于模型的柔性運(yùn)動(dòng)控制算法設(shè)計(jì)

基于模型設(shè)計(jì)的工作流程如圖4 所示，其優(yōu)勢在于從需求獲取到設(shè)計(jì)、實(shí)現(xiàn)和集成，模型始終占據(jù)開發(fā)流程的核心。其中，測試和驗(yàn)證貫穿整個(gè)開發(fā)周期，能夠方便地實(shí)現(xiàn)對(duì)算法的早期驗(yàn)證，降低后期維護(hù)成本；同時(shí)，工作流程中大量費(fèi)時(shí)及易出錯(cuò)的步驟均可實(shí)現(xiàn)自動(dòng)化。本文采用基于模型設(shè)計(jì)為柔性運(yùn)動(dòng)控制算法從模型創(chuàng)建到集成實(shí)現(xiàn)，設(shè)計(jì)完整的工作流程。

圖4 基于模型設(shè)計(jì)工作流程Fig. 4 Workflow for model-based design

2.1 SoC 系統(tǒng)結(jié)構(gòu)設(shè)計(jì)

本文針對(duì)柔性S 形加減速運(yùn)動(dòng)控制算法的實(shí)現(xiàn)，提出了SoC 軟硬件協(xié)同設(shè)計(jì)方法，實(shí)現(xiàn)結(jié)構(gòu)如圖5所示。該系統(tǒng)結(jié)構(gòu)的軟硬件劃分主要依據(jù)：對(duì)于系統(tǒng)中實(shí)時(shí)性要求嚴(yán)格、并行處理的功能模塊采用可編程邏輯實(shí)現(xiàn)；對(duì)于靈活度較高、計(jì)算相對(duì)復(fù)雜的功能模塊采用嵌入式軟件實(shí)現(xiàn)。

圖5 柔性控制算法的實(shí)現(xiàn)結(jié)構(gòu)Fig. 5 Implementation structure of flexible control algorithm

脈沖發(fā)生器模塊是運(yùn)動(dòng)控制器伺服系統(tǒng)控制的基本接口模塊，具有可重用性和高實(shí)時(shí)性的特點(diǎn)，故采用高速率的可編程邏輯實(shí)現(xiàn)；迭代速度控制模塊負(fù)責(zé)為脈沖發(fā)生器提供可控的脈沖頻率，同樣置于可編程邏輯中，但由于該模塊受布線后關(guān)鍵路徑影響，將其運(yùn)行于1 MHz；參數(shù)計(jì)算單元用于處理靈活多變的接口參數(shù)，其內(nèi)部涉及多次多項(xiàng)式乘除運(yùn)算，由嵌入式軟件實(shí)現(xiàn)最佳。

2.2 軟件模型設(shè)計(jì)

參數(shù)計(jì)算單元的作用是根據(jù)接口參數(shù)列表辨識(shí)S 形速度曲線類型并計(jì)算核心參數(shù)以及各階段運(yùn)動(dòng)位移，進(jìn)而將參數(shù)以總線形式傳輸給硬件部分，程序設(shè)計(jì)流程如圖6 所示?？梢钥闯?，該設(shè)計(jì)流程的支路數(shù)與表1 的分類相對(duì)應(yīng)。

圖6 參數(shù)計(jì)算單元的程序設(shè)計(jì)流程Fig. 6 Program design flowchart of parameter calculation unit

在模型設(shè)計(jì)中，軟件模型的設(shè)計(jì)步驟如下：

（1）在M 腳本文件中以函數(shù)形式編寫參數(shù)計(jì)算單元代碼；

（2）使用MATLAB Coder 工具箱將腳本代碼轉(zhuǎn)變?yōu)镃 語言函數(shù)；

（3）在Simulink 中采用Stateflow 圖形狀態(tài)機(jī)搭建命令控制模型并在狀態(tài)轉(zhuǎn)換中調(diào)用生成的C 語言函數(shù)。

圖7 示出了完整的軟件設(shè)計(jì)模型。左側(cè)各端口表示軟件模型的輸入信號(hào)，包括命令控制、接口參數(shù)以及狀態(tài)反饋信號(hào)，均直接傳遞給Stateflow 模塊。從細(xì)節(jié)圖可以看出，Stateflow 中包含多個(gè)狀態(tài)轉(zhuǎn)移，其中黑色箭頭所指位置即為生成的函數(shù)調(diào)用，表示由外部命令控制以條件觸發(fā)方式從等待狀態(tài)遷移至加減速狀態(tài)執(zhí)行的動(dòng)作，該過程中函數(shù)僅執(zhí)行一次即可獲得計(jì)算參數(shù)，繼而將得到的參數(shù)值傳遞給輸出接口并伴隨加減速狀態(tài)的啟動(dòng)信號(hào)。

圖7 軟件設(shè)計(jì)模型Fig. 7 Software design model

2.3 硬件模型設(shè)計(jì)

由圖5 可知，硬件模型由脈沖發(fā)生器和迭代速度控制模塊組成，均具有可重用性的特點(diǎn)，但兩者并非運(yùn)行于同一時(shí)鐘下。脈沖發(fā)生器用于產(chǎn)生脈沖信號(hào)以驅(qū)動(dòng)伺服系統(tǒng)，由于脈沖頻率對(duì)應(yīng)于電機(jī)轉(zhuǎn)速，因此脈沖頻率的精度至關(guān)重要，即系統(tǒng)時(shí)鐘頻率越高，脈沖頻率越精確。迭代速度控制模塊運(yùn)行于系統(tǒng)時(shí)鐘頻率經(jīng)過采樣后的時(shí)鐘下，其運(yùn)行頻率可表示為脈沖發(fā)生器輸出的最高動(dòng)態(tài)變化頻率，但該頻率受可編程邏輯布局布線后關(guān)鍵路徑的影響，兩者互相制約，需設(shè)置合理的過采樣系數(shù)。

2.3.1 脈沖發(fā)生器 脈沖發(fā)生器的設(shè)計(jì)采用直接數(shù)字頻率合成器（Direct digital Synthesizer, DDS）技術(shù)思想，DDS 具有頻率切換速度快、頻率分辨率高、可靠性高等優(yōu)點(diǎn)[17]。圖8 所示為脈沖發(fā)生器模塊原理圖，其內(nèi)部含累加寄存器、比較器、計(jì)數(shù)器等組件，實(shí)現(xiàn)脈沖生成與計(jì)數(shù)功能。輸入信號(hào)In 是迭代速度控制模塊提供的速度值；輸入時(shí)鐘頻率Clk 對(duì)應(yīng)系統(tǒng)工作頻率，即100 MHz；計(jì)數(shù)信號(hào)Cnt 作為迭代控制模塊的反饋信號(hào)；脈沖信號(hào)Pul 分配至控制器IO 引腳與伺服驅(qū)動(dòng)器相連以驅(qū)動(dòng)電機(jī)旋轉(zhuǎn)。

圖8 脈沖發(fā)生器模塊原理圖Fig. 8 Schematic diagram of pulse generator module

在硬件模型的設(shè)計(jì)中，定點(diǎn)化設(shè)計(jì)是必不可少的步驟，通過引入定點(diǎn)化設(shè)計(jì)能夠進(jìn)一步提高輸出脈沖頻率的精度。若設(shè)定輸入信號(hào)采用32 位位寬4 位小數(shù)數(shù)據(jù)長度的定點(diǎn)數(shù)據(jù)類型，其輸出脈沖頻率范圍為0～67 MHz，頻率分辨率可達(dá)0.0625 Hz，足以滿足任何伺服驅(qū)動(dòng)器控制需求。

2.3.2 迭代速度控制 迭代速度控制是算法模型的核心部分，內(nèi)含狀態(tài)機(jī)與速度計(jì)算兩個(gè)子系統(tǒng)。狀態(tài)機(jī)的作用是依據(jù)各段位移及當(dāng)前位置切換速度運(yùn)行的階段，以狀態(tài)變量形式控制速度計(jì)算模塊。在模型設(shè)計(jì)中，速度狀態(tài)機(jī)由Stateflow 實(shí)現(xiàn)，如圖9 所示。狀態(tài)機(jī)設(shè)計(jì)為Mealy 型，其中的7 個(gè)子狀態(tài)對(duì)應(yīng)于S 形速度曲線中的7 個(gè)階段。在實(shí)際應(yīng)用中，運(yùn)行的狀態(tài)需根據(jù)各階段的位移參數(shù)確認(rèn)狀態(tài)是否被激活。圖9 中，狀態(tài)檢測采取自上而下的方式，當(dāng)滿足[ ]內(nèi)的遷移條件時(shí)，目標(biāo)狀態(tài)被相應(yīng)激活；未滿足遷移條件時(shí)，執(zhí)行優(yōu)先級(jí)為2 的遷移支路，并且狀態(tài)的退出同樣取決于是否滿足遷移條件，該條件中的變量是由脈沖發(fā)生器提供的計(jì)數(shù)信號(hào)。

圖9 速度控制狀態(tài)機(jī)Fig. 9 Speed state machine model

速度計(jì)算模塊雖然運(yùn)行于過采樣后的時(shí)鐘頻率下，但唯有在脈沖發(fā)送完成后速度才會(huì)迭代更新。圖10 所示為速度計(jì)算模塊中的核心計(jì)算單元，體現(xiàn)為式(2)的模型表達(dá)形式，其中該模塊的輸出取決于狀態(tài)機(jī)提供的狀態(tài)變量?？梢园l(fā)現(xiàn)，核心計(jì)算單元中存在多個(gè)乘加運(yùn)算模塊，這些模塊是導(dǎo)致關(guān)鍵路徑延長的主要因素。因此為避免系統(tǒng)時(shí)延及資源濫用，需要將各運(yùn)算模塊映射到可編程邏輯的DSP 硬件資源，進(jìn)一步提高計(jì)算效率。

圖10 速度計(jì)算模塊核心計(jì)算單元Fig. 10 Core unit of speed calculation module

2.4 模型仿真測試

完成算法軟硬件模型設(shè)計(jì)后，執(zhí)行系統(tǒng)級(jí)互連和仿真驗(yàn)證算法模型的可行性和準(zhǔn)確性。圖11 所示為級(jí)聯(lián)后的仿真模型，控制器算法模塊中包含了除脈沖發(fā)生器外的軟硬件模型，每部分模型的執(zhí)行速率可通過Rate Transition 模塊實(shí)現(xiàn)劃分。仿真設(shè)置脈沖發(fā)生器模塊運(yùn)行于100 MHz，迭代速度控制模塊為1 MHz，軟件模型為1 kHz。

圖11 柔性運(yùn)動(dòng)控制算法仿真模型Fig. 11 Simulation model of flexible motion control algorithm

假設(shè)被控對(duì)象脈沖當(dāng)量為 1 0 pulse/mm ，當(dāng)vs=15 mm/s 、vm=200 mm/s 、tr=0.4 s、tm=0.1 s、L=100 mm時(shí)，仿真結(jié)果如圖12 所示，其中圖12（a）為位移曲線、圖12（b）為速度曲線、圖12（c）為速度狀態(tài)、圖12（d）為脈沖輸出波形。仿真結(jié)果表明，設(shè)計(jì)模型實(shí)現(xiàn)了S 形加減速算法，準(zhǔn)確地控制了步進(jìn)速度，具有良好的柔性特征。由此可知，MBD 方便地進(jìn)行了算法的開發(fā)，減少了調(diào)試以及修改的難度。

圖12 模型仿真結(jié)果Fig. 12 Model simulation results

3 平臺(tái)搭建與實(shí)驗(yàn)驗(yàn)證

3.1 模型代碼生成及SoC 實(shí)現(xiàn)

自動(dòng)代碼生成是MBD 的關(guān)鍵優(yōu)勢。一旦完成模型的設(shè)計(jì)及驗(yàn)證，便可利用Simulink 平臺(tái)提供的工具箱為目標(biāo)平臺(tái)生成相應(yīng)的代碼。Embedded Coder 工具箱為軟件模型生成C 語言代碼，將其部署在嵌入式處理器上；HDL Coder 工具箱對(duì)硬件模型生成HDL 代碼或IP 核，部署在可編程邏輯中。

本文采用的目標(biāo)平臺(tái)是以Zynq-7000 為核心的運(yùn)動(dòng)控制器，優(yōu)勢在于ARM 處理器和FPGA 可編程邏輯器件集成在一個(gè)芯片上，同時(shí)具備了ARM 處理器的靈活性以及FPGA 的配置能力，而且AXI 標(biāo)準(zhǔn)接口為ARM 與FPGA 實(shí)現(xiàn)了高帶寬、低延時(shí)的連接。

從仿真算法模型部署目標(biāo)硬件平臺(tái)的工作流程如圖13 所示。具體步驟如下：

圖13 模型部署工作流程Fig. 13 Workflow of model deployment

（1）模型兼容性檢查：確保模型符合代碼生成規(guī)則；

（2）使用HDL Coder 為脈沖發(fā)生器模型生成IP 核；

（3）在Vivado 開發(fā)環(huán)境中為系統(tǒng)創(chuàng)建參考設(shè)計(jì)工程，并在塊設(shè)計(jì)(Block design)調(diào)用脈沖發(fā)生器IP 核，系統(tǒng)最大工作頻率為100 MHz；

（4）為迭代速度控制模型生成IP 核并將其添加到參考設(shè)計(jì)工程中，此處設(shè)置過采樣系數(shù)為100；

（5）在HDL Workflow Advisor 中為參考設(shè)計(jì)生成工程并創(chuàng)建比特流文件，然后部署至FPGA 中；

（6）使用Embedded Coder 為軟件模型生成嵌入式C 代碼，迭代步長為1 ms；

（7）Simulink 設(shè)置為外部模式，代碼將自動(dòng)嵌入到Linux 參考架構(gòu)中，隨后再編譯至ARM。

3.2 實(shí)驗(yàn)驗(yàn)證

實(shí)驗(yàn)環(huán)境如圖14 所示。運(yùn)動(dòng)控制器采用基于Xilinx Zynq-7000 為核心的低成本Zedboard 開發(fā)板，板載PMOD 接口連接外部自制信號(hào)調(diào)理板以控制被控對(duì)象，被控對(duì)象由步進(jìn)驅(qū)動(dòng)器和57 式步進(jìn)電機(jī)組成。該步進(jìn)電機(jī)末端內(nèi)含1000 線增量式編碼器，在實(shí)驗(yàn)測試中可通過讀取編碼器脈沖進(jìn)而獲得測量速度。測量設(shè)備為Tektronix MSO3032 混合信號(hào)示波器，示波器探頭連接脈沖控制引腳測量輸出脈沖。同時(shí)，利用Simulink 與Linux 系統(tǒng)之間的以太網(wǎng)鏈路可實(shí)時(shí)捕捉AXI 總線中的接口信號(hào)。

圖14 實(shí)驗(yàn)環(huán)境搭建Fig. 14 Experimental setup

為驗(yàn)證MBD 的可行性以及測試軟硬件協(xié)同設(shè)計(jì)算法的有效性，在Simulink 中實(shí)時(shí)捕捉FPGA 中迭代的速度計(jì)算值，并與模型仿真結(jié)果進(jìn)行比較。輸入列表參數(shù)為vs=30 mm/s,vm=300 mm/s,tr=0.4 s ，如圖15 所示，仿真與實(shí)驗(yàn)設(shè)置啟動(dòng)時(shí)間差為0.02 s以示區(qū)分。實(shí)驗(yàn)期間，步進(jìn)電機(jī)以相應(yīng)速度旋轉(zhuǎn)，旋轉(zhuǎn)過程中通過編碼器反饋信號(hào)獲得實(shí)時(shí)的測量速度。從4 次仿真、實(shí)驗(yàn)與測量結(jié)果對(duì)比分析，仿真與實(shí)驗(yàn)的運(yùn)行曲線基本一致，測量速度符合期望速度變化規(guī)律，由此表明基于模型設(shè)計(jì)方法是可行且可靠的。圖16 示出了示波器測量脈沖控制引腳獲得的實(shí)際脈沖波形，其脈沖頻率隨期望速度的變化而變化。表2 示出了FPGA 布局布線后的硬件資源使用情況，可以看出，模型生成的IP 核占用了較少的硬件資源，因此也證明了軟硬件協(xié)同設(shè)計(jì)算法的有效性。

表2 可編程邏輯硬件資源使用情況Table 2 Programmable logic hardware resource usage

圖15 仿真、實(shí)驗(yàn)、測量結(jié)果比較Fig. 15 Comparison of simulation, experiment and measurement results

圖16 示波器測量波形Fig. 16 Waveform measured by oscilloscope

4 結(jié) 論

本文針對(duì)嵌入復(fù)雜的柔性S 形加減速運(yùn)動(dòng)控制算法提出了軟硬件協(xié)同實(shí)現(xiàn)策略，并引入基于模型設(shè)計(jì)方法加快算法的實(shí)現(xiàn)與驗(yàn)證，提高算法的通用性。利用Simulink 圖形化設(shè)計(jì)優(yōu)勢分別為軟件與硬件設(shè)計(jì)模型；通過自動(dòng)代碼生成工具為軟硬件模型生成代碼；搭配Vivado 和MathWorks 工具箱在以Zynq-7000 為核心的運(yùn)動(dòng)控制器上實(shí)現(xiàn)完整的MBD 部署。實(shí)驗(yàn)結(jié)果表明：提出的柔性S 形加減速算法的軟硬件協(xié)同實(shí)現(xiàn)策略是有效的，實(shí)現(xiàn)的算法同時(shí)具備參數(shù)變化的靈活性以及硬件實(shí)時(shí)性并且占用了較少的硬件資源；采用MBD 方法實(shí)現(xiàn)柔性運(yùn)動(dòng)控制算法的軟硬件協(xié)同設(shè)計(jì)是可行且可靠的，該方法大大縮短從需求到實(shí)現(xiàn)的設(shè)計(jì)時(shí)間，為個(gè)性化機(jī)器人柔性運(yùn)動(dòng)控制系統(tǒng)的實(shí)現(xiàn)提供了高效的設(shè)計(jì)途徑。