基于ARM和FPGA的軟體機器人的CAN總線運動控制器的設計

袁俊杰， 朱鵬程

(北方工業大學機械與材料工程學院,北京 100043)

軟體機器人有別于傳統剛體機器人,其具有眾多優點,具有生物柔性,能夠通過蠕動、爬行、扭動穿過狹小的空間實現靈活避障或者實現柔順抓取等操作,在非結構化環境中應用前景廣泛[1],于是需要一種適用于軟體機器人運動控制的運動控制器。

到目前為止,通用的運動控制器從結構上可分為以下3大類[2-3]。

(1)標準總線式運動控制器。這種運動控制器大多是基于計算機標準總線的,其相對于計算機是獨立的、具有開放式體系結構。其大多使用數字信號處理器(digital signal processor,DSP)芯片或微機芯片作為處理器,可實現運動規劃、高速插補、伺服濾波和電機驅動、I/O通用接口等功能,并且用戶可以使用其豐富的函數庫,根據自己的需求,在不同平臺下自行開發相應軟件來組成控制系統。

(2)開放式運動控制器。這種運動控制器靈活性好,將控制軟件安裝在計算機中,再加上伺服驅動器、通用I/O接口等硬件電路,就可以在操作系統支持下,利用開放式的運動控制器內核,開發所需要的各種類型的高性能運動控制系統,因此它可以提供給用戶更多的選擇。其優點是開發設計成本相對較低,可以給用戶提供更加個性化的開發平臺。

(3)嵌入式運動控制器。這種運動控制器能夠獨立運行,其特點是把計算機嵌入到運動控制器中。而它們之間的通信則是依靠計算機總線,該總線實質上是計算機標準總線一種變化。對于標準總線來說,其采用了更加通用的總線連接方式,在工業生產中有更廣泛的應用。在實際使用中,采用如控制器局域網絡 (controller area network，CAN)總線、以太網、串口等現場通信接口來連接計算機或控制面板以實現對該種運動控制器的上位機控制。

鑒于以上總結分析,對基于STM32和FPGA(field-programmable gate array)的嵌入式運動控制器進行了研究和設計,該嵌入式硬件架構能充分發揮它們的優點,可以同時控制多個電機,目前很少應用在軟體機器人控制中。對于二者之間的通信則采用可變靜態存儲控制器(flexible static memory controller,FSMC)總線來實現數據信息的交換與傳輸,并采用CAN總線來實現與上位機通信,該總線具有實現連接方便、靈活度高、實時性強等特點。

1 總體方案設計

控制器以基于Cortex-M4處理器為核心的STM32F407芯片進行通信、數據處理等操作,它具有多種集成功能,滿足系統的設計要求。FPGA作為該控制器的脈沖發生和數據采集機構,則利用其高速性和并行性進行速度控制、位置確定和算法實現。控制器的總體構造如圖1所示。

圖1 總體方案結構Fig.1 Overall schematic

控制器主要由上位機、STM32開發板控制模塊、FPGA開發板脈沖發生模塊、伺服驅動器等部分組成。其中每個部分的組成和相應的作用是:上位機具有信息傳輸和CAN通信的作用，CAN通信具有大量數據高速通信的優點,通過上位機可向STM32開發板發送指令,如實現電機的啟動、停止、正反轉等；STM32開發板控制模塊與FPGA開發板脈沖發生模塊,通過I/O接口來進行兩者之間的數據發送和讀取，STM32開發板根據上位機傳輸過來的的不同命令和相關數據進行對應的數據分析和處理,然后傳輸給FPGA開發板,與此同時也要從FPGA開發板上依次讀取直流伺服電機在運動過程當中相應的速度和位置等參數；伺服驅動器通過脈沖寬度調制來實現直流伺服電機的精確控制,如正反轉、加減速、啟動、停止等。控制器采用直流有刷伺服電機,其具有成本較低、結構簡單、控制方便、啟動轉矩大、調速范圍寬等優點，因此適合作為控制器的運動執行部分[4-5]。

2 硬件設計

在對控制器進行硬件設計時,其核心器件有STM32開發板、FPGA開發板、伺服驅動器、CAN通信模塊和外圍電路等。考慮到在制作和使用過程中的成本和性能方面,現對各個模塊進行選型,以達到合理與合適的效果。其中STM32開發板選擇基于Cortex-M4內核的STM32F407IGT6,FPGA開發板則選用Xilinx公司的AX7102,直流伺服電機選擇MAXON MOTOR RE30。

每個開發板的最小系統能夠正常運行是硬件設計的基礎,而每個最小系統都是由很多模塊組成的,且都有對應的功能。電源模塊為該系統提供電源，晶振模塊為該系統提供基本的時鐘信號，下載模塊則負責將程序下載到該系統中，I/O接口模塊是為了進行數據的輸入和輸出，復位模塊的作用是使CPU恢復到原始狀態，等等。不同的是,FPGA的最小系統中芯片外圍濾波電路模塊的功能是濾去電壓中的紋波、配置電路模塊以達到實現相應的功能、存儲模塊用以數據存儲等。STM32的最小系統則還有通信模塊以實現上位機與開發板之間的通信。下面就下載模塊、接口模塊、通信模塊進行詳細描述[6]。

2.1 STM32模塊電路

2.1.1 仿真器接口模塊

STM32有2種下載模式,分別是串行調試(SWD)模式和聯合測試工作組(JTAG)模式。本文設計的系統采用JTAG模式，如圖2所示。JTAG作為一種邊界掃描技術,主要被用來做芯片內部測試。控制系統設計時選用的JTAG調試口針數為20針。由于開發板預留的是SWD接口,則通過ST-Link實現JTAG與SWD的轉換,這里SWD接口的頻率使用4 MHz。在進行代碼調試時,通過ST-Link將上位機與STM32微控制器開發板相連,進而燒寫程序。

圖2 仿真器接口模塊Fig.2 Simulator interface module

2.1.2 CAN通信模塊

CAN是一種異步通信,只有CAN_High和CAN_Low兩條信號線,共同構成一組差分信號線,以差分信號的形式進行通信。由于上位機與STM32通過CAN進行通信需要USB轉CAN模塊[7],這里選用若比特公司的USB2CAN適配器V2,其工作原理如圖3所示。

圖3 CAN通信原理Fig.3 Principle of CAN communication

STM32開發板具有內置的CAN控制器,即bxCAN,它支持CAN協議2.0A和CAN協議2.0B,具有3個發送郵箱,2個3級深度的接收FIFO(first input first output,先進先出),14個可變位寬的過濾器組,波特率最高為1 Mbit/s[8]。CAN通信模塊如圖4所示。

圖4 CAN通信模塊Fig.4 CAN communication module

2.2 STM32與FPGA接口模塊

STM32開發板與FPGA開發板的接口電路的作用是實現二者之間的通信以及數據傳輸。在控制器設計過程中,控制信號、地址信號和伺服電機運行參數是STM32開發板與FPGA開發板之間相互傳輸的主要數據。在輸入/輸出端口中有4組引腳被使用到,分別是PD、PE、PF、PG中的部分引腳,接口模塊的具體設計如圖5所示。

圖5 STM32與FPGA接口模塊Fig.5 Interface module between STM32 and FPGA

在該設計中,FSMC總線用來實現STM32開發板和FPGA開發板之間數據的傳輸,所以PE0、PE1、PD4、PD5、PG10作為FSMC總線端口。除此以外,STM32還將部分端口配置為步進電機驅動器方向信號的輸出端口,FPGA的脈沖輸出信號輸入至步進電機驅動器。

2.3 伺服驅動器

作為自動調速系統中的一部分,伺服驅動器是用來調節電動機的轉速的[9]。采用的是COPLEY公司的Accelnet型伺服驅動器。該驅動器的組成部分有電源電路、繼電器板、主控板、驅動板及功率器件。伺服驅動器的工作原理如圖6所示。

圖6 伺服驅動器工作原理Fig.6 Working principle of servo driver

電源電路通過把外部輸入的20～55 V直流電轉換為相應的直流電,為繼電器板、主控板、驅動板和功率器件提供直流電源。繼電器板的作用包括兩個方面,一方面是提供相應的直流電,另一方面是完成控制信號、轉速檢測信號和轉子位置檢測信號之間的傳遞。該伺服驅動器的核心部分是主控板,主控板則采用數字信號處理器作為控制核心[10]。功率器件采用智能功率模塊(intelligent power module,IPM)為核心的驅動電路,即主電路,采用三相全控橋式逆變電路,在主回路中還加入軟啟動電路,以減小啟動過程對驅動器的沖擊。

3 軟件設計

在軟件設計部分主要涉及3個模塊,分別是STM32與上位機通信模塊、FPGA與STM32通信模塊和直流電機運動中的加減速模塊,如圖7所示。

3.1 STM32與上位機通信模塊

關于STM32與上位機的CAN通信在硬件設計部分已經介紹過了,這里只對軟件設計進行詳細介紹,軟件部分是基于CAN總線來實現STM32與上位機的通信,主要任務是只要進行CAN初始化和數據發送即可。本設計初始化模塊中只需要對 CAN 工作方式進行配置,而不需要對過濾器進行特殊的配置,部分初始化程序如下。

/* CAN register init(CAN寄存器初始化)*/

CAN_DeInit();

CAN_StructInit(&CAN_InitStructure);

/* CAN cell init(CAN單元初始化)*/

CAN_InitStructure.CAN_TTCM=DISABLE;

……

CAN_InitStructure.CAN_Mode=CAN_Mode_Normal; //工作模式選擇

CAN_InitStructure.CAN_SJW=CAN_SJW_1tq;

CAN_InitStructure.CAN_BS1=CAN_BS1_5tq;

CAN_InitStructure.CAN_BS2=CAN_BS2_1tq;

CAN_InitStructure.CAN_Prescaler=6;//波特率的設定

CAN_Init(&CAN_InitStructure);

/* CAN filter init(CAN過濾器初始化)*/

CAN_FilterInitStructure.CAN_FilterNumber=0;

……

CAN_FilterInitStructure.CAN_FilterActivation=ENABLE;

CAN_FilterInit(&CAN_FilterInitStructure);

其中波特率的配置在工作方式配置中是最主要的,設計中使用的開發板中CAN是掛APB1時鐘上的,其頻率為42 MHz。由波特率的計算公式，波特率=1/正常的位時間,正常的位時間=1tq+tBS1+tBS2。但是這樣計算的為理論值,且難以計算和理解,在實際設置中有簡單公式(1),現欲得到1 Mbit/s的波特率,計算如下:

式(1)中：CANclock為APB總線頻率;prescler為APB總線時鐘6分頻;tq、tBS1、tBS2分別對應1個、5個和1個tcan時鐘周期

3.2 FPGA與STM32通信模塊

在硬件設計中已經介紹了STM32和FPGA使用I/O口連接,軟件設計采用FSMC總線實現二者的數據信息的交換。

STM32和FPGA的通信既要完成STM32向FPGA 下傳數據的工作,還要達到STM32從FPGA中回讀數據的目的。所以兩者的通信包含了接收數據和傳送數據兩個部分。在本文所設計的控制系統中,為了能夠實現同步控制多臺步進電機,使用了FSMC來實現STM32和FPGA兩者之間的通信。

ST公司為用戶開發提供了完整、高效的工具和固件庫,STM32固件庫中擁有FSMC的SRAM控制器相應的操作固件,其中含有1個數據結構和3個函數。

FSMC_NORSRAMInitStructure;//調用庫函數；

RCC_Configuration();//時鐘選擇；

NVIC_Configuration();//中斷優先級；

FSMC_GPIO_Configuration(); //連接IO口初始化；

FSMC_SRAM_Init(); //FSMC配置；

USART_Initial(); //UART1端口配置。

本設計初始化模塊中只需要對FSMC工作方式進行配置,部分STM32初始化程序如下：

FSMC_NORSRAMInitTypeDefFSMC_NORSRAMIni-tStructure;//定義FSMC初始化的結構體變量

FSMC_NORSRAMTimingInitTypeDefreadWrite-Timing; //用來設置FSMC讀時序和寫時序的指針變量

GPIO_InitTypeDefGPIO_InitStructure; //初始化FSMC總線的IO口。

部分FPGA程序如下:

assign rd=!(csn & rdn); //獲取讀脈沖

assign wr=!(csn & wrn) ; //獲取寫脈沖

assign db=rd? indata:16′hzzzz; /*當不進行讀寫操作時db=indata；

當進行寫操作時db=16′hzzzz；當進行讀操作時db=indata*/

3.3 加減速模塊

在伺服電機從穩定運行狀態轉換為停止狀態時,如果直接停止,會因為慣性發生過沖的情況。電機運行要經過啟動、加速、恒速、減速、停止的幾個階段,這其中加減速的要求是既能夠在最短時間內達到穩定或者停止的要求,又不能產生失步或者過沖的情況。

在控制電機過程中,對電機進行加減速控制常用的方法主要有梯形曲線法、S型曲線法、指數曲線法、拋物曲線法等。S型曲線加減速法是一種沖擊力小、柔性程度較好的控制方法,能讓電機性能得到充分發揮,減小沖擊振動[11]。S型曲線加減速法作用方法是通過改變運動過程中加速度的大小,使其在不同階段擁有不同的加速度,來大幅度地減小沖擊力[12]，如圖8所示。

根據加速度大小的變化,可以將加減速過程分為7段(即m1～m7)。由圖8可知速度v、加速度a、加加速度j以及勻加速階段的加速度值為A、勻減速階段的加速度值為-D,Ti(i=1,2,3，…，7)表示每個階段的運動時間,ti(i=0,1,2，…，7)為每個階段過渡點的時刻,τi(i=1,2,3，…，7)為以每個階段起始點作為零點的時間,其中τi=ti-ti-1(i=1,2,3，…，7)。加速度a、電機速度v與時間t的關系如式(2)和式(3)所示,vi(i=0,1,2，…，7)、ai(i=1,2,3，…，7)分別為m1～m7每個階段結束時的速度、加速度值。

圖8 S曲線加減速圖Fig.8 S curve acceleration and deceleration diagram

對加減速模塊進行程序設計并通過FPGA的編程軟件Vivado編程后在仿真軟件Modelsim上進行仿真,為了加快仿真速度,特意修改了部分參數,但不影響仿真結果，其仿真結果如圖9所示。由仿真結果可以看出,該加減速模塊構造滿足速度和加速度曲線的構造,滿足速度變化平穩、加速連續等條件,起始和終止速度與要求速度一致,在穩定運行階段其加速度為0,加加速度可以根據實際需要隨時調整,通用性很好。

圖9 S加減速的仿真波形Fig.9 Simulated waveform of S acceleration and deceleration

4 實驗驗證

為驗證該運動控制器的控制效果和控制精度,現對其控制進行實驗驗證。實驗驗證是在自行設計制作的運動平臺上進行的,該運動實驗平臺運動執行部分采用的是彈性比較好的彈簧,中間則是支撐彈簧,其中3個電機與3根纜繩相連,纜繩的另一端則連接在軟體機器人的單個關節上,通過控制器控制不同電機的啟停運轉,來實現該節彈簧的彎曲,并可以實現彈簧在不同方向的運動。控制器以基于Cortex-M4處理器為核心的STM32F407芯片和Xilinx公司的AX7102的FPGA開發板,進行通信、數據處理、位置確定和算法實現等操作。軟體機器人運動平臺如圖10所示。

圖10 軟體機器人運動平臺Fig.10 Software robot motion platform

通過控制算法和對應的數據計算,在運動平臺按照預先設定的運動方式進行運動。通過同時控制多個電機進行協同運動,其中電機轉速為2 r/min,以中間支撐軸為原點,可以得到在最終運動狀態下機器人彎曲了約45°,纜繩縮短約5 cm。具體運動實驗過程如圖11所示。

圖11 軟體機器人運動實驗Fig.11 Motion experiment of software robot

圖11僅展示機器人首節運動,其中圖11(a)記為機器人起始狀態,圖11(b)、圖11(c)、圖11(d)分別對應5、15、30 s時運動狀態,其中圖11(d)即為最終運動狀態。由運動狀態結果可以得到,開發板在控制電機運動過程中整體運行平穩,運動速度有變化,在按照設定的軌跡運動時運動精度較高,基本實現了設想的功能,使得該實驗成功實現。

5 結論

對于目前軟體機器人運動控制難度大、精度低、協同控制多電機難以實現同步性等問題,提出采用基于ARM和FPGA的軟體機器人的CAN總線運動控制器，得到以下結論。

(1)實現STM32與FPGA接口通信,STM32開發板根據上位機傳輸過來的不同命令和相關數據進行對應的數據分析和處理,然后傳輸給FPGA開發板,與此同時也要從FPGA開發板上依次讀取直流伺服電機在運動過程中相應的速度和位置等參數；伺服驅動器通過脈沖寬度調制來實現直流伺服電機的精確控制。

(2)實現STM32開發板與上位機之間的通信,這里采用CAN總線,因為CAN總線可以進行大量數據的高速通信,適合對多電機進行協同控制,并可大量傳輸下位機采集到的數據。

(3)在加減速模塊中,采用S型曲線加減速法有利于減小電機的沖擊,實現機器人整體平穩運行,作用方法是通過改變運動過程中加速度的大小,使其在不同階段擁有不同的加速度,來大幅度減小沖擊力。

(4)根據自行設計加工的運動對軟體機器人的運動控制進行了實驗驗證,實驗結果表明,該控制器整體協同控制性能較好,能比較理想地實現預期的運動。

由于控制算法部分并沒有介紹,在實際控制過程中與控制算法中的仿真存在誤差,所以下一步會對控制算法和機械結構進行優化,以期望實現更理想的運動。