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

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

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

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

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

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

1 柔性運動控制算法建模與仿真

1.1 S 形加減速算法的運動學(xué)建模

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

圖1 S 形加減速運動曲線Fig. 1 Motion profiles of S-shaped ACC/DEC

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

圖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替換為總加速時間tr與tm，這樣在建模過程中可有效避免三次多項式[15]的求解，從而為運動控制器減輕計算負擔(dān)。

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

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

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

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

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

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

1.2 S 形加減速算法仿真

基于先前的分析，采用MATLAB 2020a 對不同條件下的S 形加減速算法進行仿真。當(dāng)仿真數(shù)據(jù)為v0=100 mm/s ，vm=600 mm/s ，tr=0.6 s ，tm分別為0、0.15、0.3 s 時，分析不同運動路徑下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ù)下的速度曲線均存在勻速段，體現(xiàn)為梯形速度曲線、7 段S 形速度曲線和5 段S 形速度曲線。

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

2 基于模型的柔性運動控制算法設(shè)計

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

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

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

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

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

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

2.2 軟件模型設(shè)計

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

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

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

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

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

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

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

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

2.3 硬件模型設(shè)計

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

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

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

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

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

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

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

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

2.4 模型仿真測試

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

圖11 柔性運動控制算法仿真模型Fig. 11 Simulation model of flexible motion control algorithm

假設(shè)被控對象脈沖當(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時，仿真結(jié)果如圖12 所示，其中圖12（a）為位移曲線、圖12（b）為速度曲線、圖12（c）為速度狀態(tài)、圖12（d）為脈沖輸出波形。仿真結(jié)果表明，設(shè)計模型實現(xiàn)了S 形加減速算法，準(zhǔn)確地控制了步進速度，具有良好的柔性特征。由此可知，MBD 方便地進行了算法的開發(fā)，減少了調(diào)試以及修改的難度。

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

3 平臺搭建與實驗驗證

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

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

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

從仿真算法模型部署目標(biāo)硬件平臺的工作流程如圖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è)計工程，并在塊設(shè)計(Block design)調(diào)用脈沖發(fā)生器IP 核，系統(tǒng)最大工作頻率為100 MHz；

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

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

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

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

3.2 實驗驗證

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

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

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

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

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

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

4 結(jié) 論

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