基于IQMath自適應速度前瞻算法的設計與優化*

彭 鵬，胡 毅，于 東，陳智殷，陸小虎

(1.中國科學院大學 研究生院，北京 100049；2.中國科學院沈陽計算技術研究所 高檔數控國家工程研究中心，沈陽 110168；3.沈陽高精數控技術有限公司，沈陽 110168)

基于IQMath自適應速度前瞻算法的設計與優化*

彭 鵬1,2，胡 毅2,3，于 東3，陳智殷1,2，陸小虎1,2

(1.中國科學院大學 研究生院，北京 100049；2.中國科學院沈陽計算技術研究所 高檔數控國家工程研究中心，沈陽 110168；3.沈陽高精數控技術有限公司，沈陽 110168)

自適應速度前瞻控制算法作為數控系統實現高精、高速加工控制的核心算法，其運行效率以及運算結果的精度直接影響著整個工件的加工效率以及加工質量，文章基于ARM+DSP構建的雙核架構嵌入式數控系統，研究和分析了自適應速度前瞻控制算法實現的基本原理、IQMath庫的優化原理、以及線性匯編級的優化方法，在此基礎上設計并實現了自適應速度前瞻控制算法，針對實驗室嵌入式數控系統定點DSP的特點，對該算法先后進行了基于IQMath庫的優化，以及線性匯編級的優化，并對優化前與優化后算法進行了性能評估以及誤差分析，結論表明優化后的算法在性能上較好地滿足了嵌入式數控系統對實時性的要求。

自適應；前瞻算法；IQMath；線性匯編

0 引言

IQMath庫是TI公司編寫的一個高優化、高精度的數學函數庫，主要是針對定點DSP，由于定點DSP中沒有浮點運算單元FPU的支持，因此在定點DSP中做浮點運算是通過軟件模擬的方式實現，其效率比軟低，IQMath庫主要是將C語言編寫的含有浮點運算的程序，轉換成定點運算，IQMath的使用能夠獲取比標準ANSI C語言更快的執行速度。

數控系統作為數控機床實現高精、高速、高質量加工的控制核心，其內部運行的運動算法的優劣直接影響著數控機床的加工質量與加工效率，并且一直是評價數控系統加工質量的重要指標。數控系統中運動軌跡的復雜多變，軌跡銜接點處的進給速度的處理，是衡量數控系統運動控制算法優劣的重要指標，如果處理的不好，很容易造成系統的抖動，對機床具有很大的沖擊，因而會降低工件的加工精度以及加工效率。自適應速度前瞻控制算法采用前瞻思想，對拐點處的速度提前進行預估，將刀具的速度提前降下來，避免了刀具速度的突變，對系統穩定至關重要；因此對該算法進行性能上的優化具有非常重要的意義。

針對上述問題，本文基于OMAP3530構建的嵌入式數控平臺，研究了IQMath庫的優化原理、匯編級的優化、自適應速度前瞻算法的設計思想，設計并實現了自適應速度前瞻算法，并對算法進行了基于IQMath庫的優化以及匯編級的優化，對優化前與優化后的算法在上述兩種平臺上分別進行了性能測試。

1 Q格式的數據類型

浮點數在定點DSP上的運算一般采用兩種方式進行: ①直接采用浮點(float)格式來表示浮點數，采用float格式表示的浮點數，在定點DSP上的運算采用的是軟件模擬的方式進行的，例如在TMS320C64X+上進行浮點乘法，DSP內部是通過_mpyf函數實現的，而對于定點乘法，則有專門的乘法指令MPY在2個時鐘周期內完成，因此在定點機上直接使用float類型進行浮點數的運算影響算法的性能；②浮點數放大，通過將浮點小數放大若干倍后變成整數，然后在定點DSP上以定點數的形式運算，通過浮點放大變成整數的方式可以提高算法的運行效率，但是在算法的移植性方面效果不佳。

為了提高定點DSP在浮點運算方面的性能以及可移植性，TI公司專門編寫了進行浮點運算的IQMath庫，IQMath庫中函數的輸入與輸出一般都是32位的定點數，根據定點數小數點位置的不同，可以將定點數分成32類，在IQMath庫里面稱為Q格式，其變化范圍為Q0～Q31，其中下標表示定點數的小數點位置，Q格式表示的定點小數如公式(1)所示，其中S表示符號位，Ik(k∈{0≤k≤30})表示第k位的二進制數。

(1)

由于定點數小數點位置的固定，因此Q格式的數據類型表示精度也隨著小數點位置的不同而不同，如文獻[1]中所示的不同Q格式數據類型的精度取值范圍，從中可以看出Q格式數據類型隨著下標的增大，表示的精度越來越高，但是表示的范圍卻越來越小。

2 IQMath庫的優化原理

2.1 內聯函數與宏定義的使用

C語言中過多的函數調用，雖然可以提高程序的結構化與規范化，但是會降低程序的運行效率，函數調用本身就是一個復雜的過程，調用的過程中涉及到函數調用上下文環境的交換，函數參數的傳遞，堆棧的操作等一系列復雜的過程，如果一個算法中涉及到太多的函數調用，以及函數的嵌套調用，勢必會影響到算法的運行時間以及算法的空間開銷，內聯函數類似于宏定義，都是在調用處對代碼進行展開，替換掉調用處的代碼，只不過一個處于預編譯階段，而另一個處于編譯階段，IQMath庫中大量的使用inline、define關鍵字，避免由于函數調用造成的程序運行時間的開銷，如IQ類型之間的轉化，浮點類型到Q格式的轉換函數：

#define_IQtoIQ31(A)((I32_IQ)(A)<<(31-GLOBAL_Q))

static inline I32_IQ _FtoIQN(float input, U32_IQ qfmt)

2.2 數據表的使用

IQMath庫為了提高程序的運行效率，將一些常用的函數值打包成靜態庫的形式，在程序加載時，直接加載到內存中，函數在運算的過程中，通過查表的形式，可以顯著提高程序的運行效率,如sin(x)的計算是通過Taylor展開式在x0=0處展開計算的，其每一項的系數均為常數：

2.3 數據類型的轉化

2.4 其他技術的使用

IQMath提供的都是靜態庫，相比于動態庫的好處，程序在編譯的時候，將靜態庫鏈接到目標代碼中，程序在運行的過程中，不再需要此靜態庫，而動態庫是在程序運行的時刻才被載入，因此在速率上不如靜態庫，將相關的函數改成與特定DSP架構相關的匯編語言或線性匯編語言實現，速度上比高級語言執行的要快。

3 鏈接器配置文件

鏈接器配置文件確定了程序鏈接成最終可執行代碼時的選項，其中有很多的條目，實現不同方面的選擇，最為重要的就是為可執行文件進行存儲器的分配，如圖1所示，其中MEMORY指定的是存儲空間布局，SECTIONS段指定的是目標文件中相關段的在存儲空間的分配位置，高級語言編寫的程序生成的目標文件一般有以下幾個段級成(.cinit程序中變量初值和常量，.const存放程序中的字符串常量、浮點常量、const聲明的常量，.switch存放switch語句的跳轉表，.text存放代段，.bss為全局變量與靜態變量保留存儲空間)。

圖1 CMD文件內存配置

由于RAM的訪問速度要快于ROM，因此在使用IQMath庫時可以將IQMath的相關函數查詢表以及數據表通過鏈接配置文件在程序運行時直接加載進RAM中，以提高算法的性能，如：

.data:IQmathTables :>ERAM

.data:IQmath :>ERAM

4 線性匯編與匯編

實現DSP算法一般分為3個階段，第一階段產生C代碼，第二階段優化C代碼，第三階段編寫線性匯編代碼，相關程序的編寫流程如下圖2如示，通過C語言編寫的程序由于未能充分考慮DSP內部架構的特點，因此在性能上往往達不到要求，這時可以考慮使用線性匯編或者匯編語言進行代碼的改寫，線性匯編與匯編類似，它的擴展名必須是.sa,用做匯編優化器的輸入文件，線性匯編代碼的編寫無需考慮寄存器的分配、功能單元的分配、指令的并行性等，匯編優化器在優化線性匯編代碼時僅對偽指令對.proc與.endproc之間的代碼進行優化。

圖2 DSP程序的編譯流程

5 自適應速度前瞻算法

以直線加減速為例，對連接微小路徑段的速度銜接建立數學模型，分析出其速度變化的規律，以實現微小路徑段之間速度的高速銜接，一條路徑段的加工一般有加速，勻速，減速中一種或幾種情況(如圖3所示)，圖中加速，勻速，減速階段的位移長度分別為S1、S2、S3，vs、ve、vmax分別代表任一路徑段的起始速度、終止速度、以及數控系統允許的最大速度，假設數控系統中允許的最大加速度以及插補周期為amax、T，某一路徑段的長度為L,根據位移、加速度、速度之間的物理模型可以計算出速度的臨界值vm,如公式(2)所示。

(2)

圖3 刀具的運動學模型

數控機床在加工的過程中，軌跡銜接點之間有一些硬性條件對速度進行了限制，因此機床在加工的過程中，刀具的運動速度必須要小于此速度的限制，假設此速度限制為vx，由文獻[2,4,7]可以得出此速度的限制公式(3)。

(3)

自適應前瞻算法的關鍵點主要在于前瞻段數的確定，算法的思想如文獻[2]所示，算法的程序流程圖如文獻[11]所示。

6 前瞻算法的優化

數控系統主要是對刀具運動軌跡的精確控制，因此算法主要是針對浮點數進行運算，在定點DSP中，由于沒有浮點運算單元FPU的支持，因此浮點運算在保證精度的情況下采用的是軟件模擬方式進行實現的，影響了算法的執行效率，對算法依據IQMath庫進行改寫，將浮點運算轉化成定點運算可以顯著提高算法的效率，實驗中為了保證精度，以及算法運行的正確性，選擇了Q24的數據類型。

高級語言編寫的代碼由于未能充分考慮DSP處理器的架構，因此對時間要求比較苛刻的算法，其性能往往達不到指定的要求，因此需要根據DSP處理器的體系結構對算法進行線性匯編級的優化，算法中由于需要多次計算坐標系中兩點之間的距離，通過C語言編寫的代碼在CCS調試運行時查看其匯編代碼數據的加載采用的是LDW指令，由于數據是連續存放的，采用LDW指令在6個時鐘周期內只能取出一個字的數據，而采用LDDW指令在6個時鐘周期內可以取出2個字的數據，增加了數據傳輸的帶寬，函數進行線性匯編改寫后的代碼如圖4所示。

圖4 三維坐標系中兩點距離的線性匯編代碼

7 實驗與性能評估

實驗采用高精數控總線實驗室(如圖5)現有的基于OMAP3530的嵌入式數控平臺，OMAP3530是TI生成的一款集成ARM 以及DSP的異構雙核高性能嵌入式處理器，實驗中編程給定的最大進給速度vmax=6mm/s，插補周期為2ms，系統允許的最大加速度amax=20mm/s2，最大輪廓誤差為ecmax=0.001mm，實驗中測試的運動軌跡由 25個插補坐標點構成，對前面的自適應前瞻算法分別進行了優化前與優化后算法的性能測試。

圖5 OMAP3530嵌入式數控系統平臺

實驗結果主要以算法的運行時間以及運算時產生的誤差作為衡量標準，為了保證實驗的精度，每次實驗均以算法運行1000次的結果作為分析，如下圖6所示為算法在未優化前的執行時間圖，圖7為算法采用IQMath庫優化后的執行時間圖，圖8為算法在IQMath庫優化的基礎上采用線性匯編再次優化后的執行時間圖。

圖6 未優化前算法執行時間圖

圖7 IQMath優化后算法執行時間圖

圖8 線性匯編優化后算法執行時間圖

由實驗結果可知優化前與優化后的算法之間執行時間最大相差42.68498ms,算法執行1000次時未優化的算法期望執行時間是43.6841ms,采用IQMath庫優化后的算法期望執行時間1.2248ms,對關鍵代碼進行線性匯編改寫后算法的期望執行時間是1.07738ms，表1為優化前與優化后算法執行1000次時的平均時鐘周期數。

表1 算法執行的時鐘數對比

算法的誤差是以算法執行過程中對刀具的速度規劃進行衡量的，相同時間點優化前與優化后規劃的速度之間的差值即為使用IQMath庫優化算法后產生的誤差，使用IQMath庫優化前與優化后算法的誤差為5.164×10-5，由于線性匯編級的優化是在IQMath庫的基礎上做的，因此對算法的速度規劃結果不產生影響，誤差值保持不變。

嵌入式數控系統OMAP3530在ARM運行的是EMC2,適應前瞻速度控制算法運行在DSP端，為了保證系統的實時性需求，ARM端將軌跡點寫入緩存，DSP端從緩存中取出數據，并進行運算，為了保證系統的實時響應，系統能允許的極限時間為20ms,因此優化后的算法能夠很好的滿足系統的實時性需求。

8 結論

自適應速度前瞻控制算法作為數控系統實現高精、高速加工控制的核心算法，其運行效率以及精度直接影響著整個工件的加工效率以及加工質量，本文基于OMAP3530構建的雙核架構嵌入式數控系統，研究和分析了IQMath庫的優化原理、線性匯編級的優化、自適應前瞻算法實現的原理，設計并實現了自適應前瞻算法，基于IQMath庫以及線性匯編的前瞻算法，對優化前與優化后的算法進行了性能評估，對于目前的高精、高速數控加工具有一定的的理論與實際意義。

[1] Texas Instruments.TMS320C64X+ IQmath Library User’s Guide.2008 December.

[2] 徐志明，馮正進，汪永生.連續微小路徑段的高速自適應前瞻插補算法[J].制造技術與研究,2003(12):20-22.

[3] 于東，周雷，黃燕，等.微小程序段的動態前瞻處理方法及實現裝:中國，ZL200510047751.X[P].2005,11,18.

[4] 王海濤，越東標，高素美.CNC自適應速度前瞻控制算法的研究[J].機械科學與技術.2009(3):346-349.

[5] 彭志明,李琳.基于IQmath庫的定點DSP算法設計[J].單片機與嵌入式系統應用,2010(9):39-41.

[6] 李珊珊.嵌入式彈載計算機性能優化與實現[J].航空兵器,2011(2):56-59.

[7] 張曉輝.數控系統高速高精運動軌跡控制技術的研究與實現[D].北京：中國科學院研究生院，2011.

[8] 鄭回青，林嘉宇，張鑌.基于TMS320C64x DSP的匯編優化方法[J].微處理機，2010(2):105-108.

[9] 宋巍.基于TMS320DM6446的AVS編碼器的實現[D].太原：太原理工大學，2008.

[10] 胡毅，于東，李培楠，等.基于現場總線的開放式數控系統的設計與實現[J].小型微型計算機系統，2008,29(9):1745-1749.

[11] 馮強，胡毅，于東，等.基于多核嵌入式數控系統的速度前瞻算法研究[J].組合機床與自動化加工技術，2013(3):16-20.

(編輯 李秀敏)

Speed Look-ahead Algorithm Design and Optimization Based on IQMath

PENG Peng1,2，HU Yi2,3，YU Dong3，CHEN Zhi-yin1,2，LU Xiao-hu1,2

(1. University of Chinese Academy of Sciences, Beijing 100049，China;2. National Engineering Research Center For High-End CNC, Shenyang Institute of Computing Technology, Chinese Academy of Sciences, Shenyang 110168,China)

Execution efficiency and The accuracy of the operation result have directly impact on the processing efficiency and processing quality of the workpiece, Based on ARM&DSP dual-core embedded CNC system, This paper studies and analyzes the basic principals of CNC adaptive speed look-ahead algorithm,the optimization theory of the IQMath Library, and the linear assembly level optimization methods, Base on the above theory,design and implement adaptive look-ahead control algorithm, For the characteristics of the laboratory’s fixed -point DSP embedded CNC system,adopt IQMath library and linear assembly to optimize the algorithm,at last,the performance evaluation and error analysis of un-optimized and optimized algorithm are made.

adaptive; speed look-ahead algorithm ; IQMath; linear assembly

1001-2265(2014)07-0049-04

10.13462/j.cnki.mmtamt.2014.07.014

收稿日期：2013-11-13；修回日期：2013-12-19

高檔數控機床與基礎制造裝備國家科技重大專項：基于二次開發平臺的專用數控系統開發與應用(2013ZX04007-011)

彭鵬(1988—)，男，河南信陽人，中國科學院大學碩士研究生，主要研究方向為數控技術，(E-mail)pengpeng03@baidu.com。

TH166;TG65

A