基于CAN 總線的分布式船用六自由度穩(wěn)定平臺的同步控制系統(tǒng)設計

鄭文彬，曹 凌，代 磊，信光成，陳銘堃

（九江精密測試技術(shù)研究所，九江 332000）

1 前言

船用穩(wěn)定平臺，是能隔離船舶波浪運動保持物體在慣性空間下方位不變的裝置，在軍事和民用領域發(fā)揮著重要作用：在軍事方面，穩(wěn)定平臺可以隔絕船體干擾，提高艦載武器瞄準精度，實現(xiàn)精確瞄準和對目標的運動跟蹤；在民用方面，可用于船上穩(wěn)定手術(shù)室、雷達裝置、直升機降落平臺等領域。隨著我國建設海洋強國戰(zhàn)略步伐的不斷邁進，對高精度船用穩(wěn)定平臺的需求變得更為迫切，船用穩(wěn)定平臺控制系統(tǒng)的研究有著廣闊的應用前景，目前已成為高校和科研機構(gòu)的研究熱點[1]。

Stewart 平臺是目前使用廣泛的六自由度平臺，由上臺面、下臺面、六個運動缸以及6 個虎克鉸組成。平臺可進行6 個自由度運動：橫滾、偏擺、俯仰、橫移、前后和升降[2]。因其在六個維度上的運動與船舶的波浪運動相同，常用作海上穩(wěn)定平臺。

穩(wěn)定平臺姿態(tài)調(diào)整的步驟；設定平臺目標姿態(tài)；通過姿態(tài)反解求得各運動缸的目標位移；在控制器的作用下，使6 個運動缸同步平穩(wěn)的運動到目標位置，完成平臺的姿態(tài)調(diào)整[3]。

由于6 個運動缸需同步運動，目前廣泛使用EtherCAT 總線對運動缸進行同步控制。由于Ethercat 使用了非標準的鏈路層，需要使用專用芯片來處理協(xié)議，相比于基于通用協(xié)議（比如TCP/IP/CAN）硬件成本較高[4]，本文提出了一種在工業(yè)領域應用廣泛、安全可靠的基于CAN 總線的分布式六自由度平臺同步控制系統(tǒng)。

1 基于CAN 總線的分布式控制系統(tǒng)

穩(wěn)定平臺在進行姿態(tài)運動時，需進行姿態(tài)反解以及六個運動缸的閉環(huán)控制，因此建立如圖1 所示的分布式控制系統(tǒng)，該系統(tǒng)包括一個主控制器、六個從控制器、通信總線、同步脈沖信號：主控制器主要對平臺進行整體控制，用以實現(xiàn)運動反解、人機交互、參數(shù)存儲和更改等功能；從控制器分別控制運動缸的閉環(huán)運動，實現(xiàn)運動缸平穩(wěn)運動控制、狀態(tài)上報等功能；同步脈沖信號用以協(xié)調(diào)系統(tǒng)同步運行；通信總線用以連接主控制器和從控制器，實現(xiàn)主控制器的控制指令下發(fā)和從控制器的運動狀態(tài)上報，通信總線采用CAN 總線。

圖1 平臺控制系統(tǒng)

CAN 是一種有效支持分布式控制和實時控制的串行通信網(wǎng)絡，具有通信速率高、實現(xiàn)容易、性價比高等優(yōu)點：CAN 總線的通訊方式靈活，可通過報文濾波實現(xiàn)點對點、一點對多點及全局廣播等方式傳送接收數(shù)據(jù)，提供高達1Mbps 的數(shù)據(jù)傳輸速率；CAN 總線采用短幀格式通信，傳輸時間短，受干擾概率低，具有極好的檢錯效果，每幀信息都有CRC 校驗及其他檢錯措施，從而保證了數(shù)據(jù)通信的可靠性；CAN 總線采用非破壞性總線仲裁技術(shù)，大大節(jié)省了總線沖突仲裁時間，在網(wǎng)絡負載很重的情況下也不會出現(xiàn)網(wǎng)絡癱瘓情況。

同步脈沖信號為主控制器定時產(chǎn)生并發(fā)出高低電平脈沖信號，通過電纜傳輸至各從控制器上，從控制器在接收到同步脈沖信號時進行運動缸的伺服控制。由于同步脈沖信號是由統(tǒng)一由主控制器發(fā)出，因此各從控制器控制的運動缸得以保持運動同步。

2 控制器硬件設計

2.1 控制器CPU 選型

控制器采用德州儀器（TI）公司生產(chǎn)的DSP 芯片TMS320F28 335（28 335）作為核心CPU，它是一款高性能的32 位CPU，包含單精度浮點運算單元(FPU)，采用哈佛流水線結(jié)構(gòu)，主頻最高可達150 MHz，具有強大的運算處理能力；該芯片外設豐富，具有18 路PWM 功能輸出模塊、6 個事件捕獲輸入、2 個通道的正交調(diào)制模塊（QEP），能夠較好地完成電機控制以及測角信號接收；該芯片支持多種通信接口，包含2路CAN 模塊、3 路UART 模塊、1 路SPI 模塊、1 個I2模塊C。因此該芯片具有強大的運算處理、電機控制以及通信能力，可以滿足項目的運算、通信和控制需求，主控制器和從控制器，均采用該芯片作為核心CPU。

2.2 電源模塊設計

TMS320F28 335 芯片需要2 種工作電壓電源：3.3 V 電壓為IO 供電電壓，1.9 V 電壓為內(nèi)核供電電壓。通過選型，電源采用TD6 821 型芯片，它可同時輸出1.9 V 電壓和3.3 V 電壓。電源模塊原理圖，如圖2 所示。

圖2 電源模塊設計

2.3 主控制器設計

主控制器主要進行姿態(tài)反解、CAN 通信、人機交互以及系統(tǒng)固有參數(shù)的存儲，包括CPU、CAN通信模塊、EEPROM 模塊、RS232 模塊，其電路原理框圖，如圖3所示：

圖3 主控制器原理框圖

1）CAN 模塊

采用TJA1040 型芯片實現(xiàn)CAN 通信收發(fā)功能；主控制器通過CAN 通信接口連接6 個從控制器，接收6個從控制器上報的運動狀態(tài)以及發(fā)送運動指令至6 個運動缸；

2）EEPROM 模塊

采用24C02型芯片實現(xiàn)系統(tǒng)固有參數(shù)的存儲功能，28 335 型芯片通過I2C 總線與24C02 芯片通信完成數(shù)據(jù)的寫入和讀??；

3）RS232 模塊

主控制器通過RS232 串口與上位機進行通信，RS232 串口采用28 335 的UART 模塊，通過SP3232芯片實現(xiàn)RS232 串口收發(fā)器功能；主控制器可通過RS232 串口接收目標姿態(tài)指令以及上報六自由度平臺的整體運行狀態(tài)；主控制器通過28335 的定時器和GPIO 引腳產(chǎn)生同步脈沖信號，并發(fā)送至各從控制器，保證6 個運動缸同步運行。

2.4 從控制器設計

從控制器主要進行運動缸閉環(huán)控制、CAN 通信以及固有參數(shù)的寫入和存儲，包括CAN 通信模塊、CPU、EEPROM 模塊以及DAC 模塊，6 個從控制器采用相同的電路原理，如圖4 所示：運動缸閉環(huán)控制需要進行電機驅(qū)動和反饋測角讀取，從控制器在收到目標位置后，通過驅(qū)動器驅(qū)動電機轉(zhuǎn)動；當電機轉(zhuǎn)動后，時刻對比目標位置和反饋位置，當反饋位置與目標位置相差較大時輸出較大控制量，當反饋位置靠近目標位置時則減小控制量，這樣經(jīng)過多個控制周期的迭代，運動缸到達目標位置；采用電機驅(qū)動器驅(qū)動電機轉(zhuǎn)動，步驟為：控制器發(fā)出控制DA 到電機驅(qū)動器，電機驅(qū)動器根據(jù)控制DA 的大小調(diào)節(jié)輸出PWM 的占空比調(diào)節(jié)電機轉(zhuǎn)速[5]；電動缸的電機上安裝有多圈角度編碼器，通過圈數(shù)和角度可計算得到電動缸當前位置；控制器采用主控制器相同的TJA1 040 芯片和24C02 芯片，實現(xiàn)CAN 通信的收發(fā)和固有參數(shù)的存儲和更改。

圖4 從控制器原理框圖

DAC 模塊采用TLV5 620 型芯片實現(xiàn)DA 轉(zhuǎn)換和輸出功能，28 335通過SPI總線與芯片通信傳輸數(shù)字信號，芯片把數(shù)字信號進行DA 轉(zhuǎn)換生成相應的DA 值輸出到電機驅(qū)動器上，電機驅(qū)動器根據(jù)輸入的DA 值，產(chǎn)生相應占空比的PWM 波驅(qū)動電機轉(zhuǎn)動。

從控制器使用TMS320F28 335正交調(diào)制模塊（QEP）與多圈角度編碼器通信，接收編碼器的圈數(shù)和角度信息，通過圈數(shù)與角度信息解算出電動缸實際位置；從控制器接收主控制器的同步脈沖信號，以同步脈沖為控制周期進行電機閉環(huán)控制。

3 控制系統(tǒng)軟件設計

3.1 主控制器軟件設計

主控制器軟件主要進行平臺姿態(tài)反解、CAN 口通信、RS232 串口通信、同步脈沖的產(chǎn)生以及系統(tǒng)固有參數(shù)的存儲；主控軟件采用前后臺的軟件結(jié)構(gòu)，前臺為主函數(shù)，后臺為中斷服務函數(shù)，中斷函數(shù)采用定時中斷方式觸發(fā)。

主控制器主函數(shù)主要進行系統(tǒng)的初始化、RS232串口數(shù)據(jù)接收以及CAN 信息的接收，程序流程如圖5所示：系統(tǒng)上電時進行DSP 系統(tǒng)時鐘初始化，再進行GPIO、CAN、I2C、SCI 模塊初始化，接著通過I2C 總線從EEPROM 中讀取固有參數(shù)，開啟初始化定時中斷，隨后通過輪巡的方式接收串口指令和CAN 口信息；串口指令由上位機傳來，主要分為姿態(tài)運動和參數(shù)存儲指令。當接收的是姿態(tài)運動指令時，置位姿態(tài)反解標志；當接收的是參數(shù)指令時，則通過I2C 總線往EEPROM中寫入?yún)?shù)。

圖5 主控制器軟件主函數(shù)程序流程圖

中斷函數(shù)主要進行同步脈沖信號的產(chǎn)生、姿態(tài)反解計算、六個運動缸的目標位置的發(fā)送以及系統(tǒng)狀態(tài)上報，程序流程如圖6 所示：首先是生成同步脈沖；接著查詢是否需要進行姿態(tài)反解，若需要進行則先根據(jù)目標姿態(tài)進行姿態(tài)反解，計算得到6 個運動缸的目標位置；隨后通過CAN 總線把目標位置發(fā)送到各個從控制器；最后通過RS232 串口發(fā)送系統(tǒng)運動狀態(tài)至上位機，退出中斷。

圖6 主控制器軟件中斷函數(shù)流程圖

3.2 從控制器軟件設計

從控制器軟件，主要進行運動缸閉環(huán)控制、CAN通信以及系統(tǒng)固有參數(shù)的存儲和更改：從控制器軟件同樣采用前后臺的軟件結(jié)構(gòu)，前臺為主函數(shù)，后臺為中斷服務函數(shù)，與主控制軟件不同的是：從控制器軟件的中斷觸發(fā)方式采用外部中斷方式，外部中斷源為主控制器產(chǎn)生的同步脈沖信號，6 個從控制器采用相同的控制軟件。

主函數(shù)主要進行系統(tǒng)的初始化、參數(shù)讀取以及指令接收，程序流程如圖7 所示：首先初始化系統(tǒng)時鐘；隨后進行GPIO、CAN、I2C 模塊初始化，讀取固有參數(shù)，開啟初始化外部中斷；接著以查詢的方式接收CAN 信息，包括參數(shù)指令和運動指令。當接收到參數(shù)指令時，通過I2C 接口往EEPROM 中寫入控制參數(shù)；當接收到運動指令時，則置位走位標志。

圖7 從控制器軟件主函數(shù)流程圖

中斷函數(shù)主要進行運動缸的閉環(huán)伺服控制和運動狀態(tài)的上報，程序流程如圖8 所示：首先利用正交調(diào)制模塊（QEP）讀取多圈角度編碼器的圈數(shù)和角度，再根據(jù)圈數(shù)和角度計算電動缸當前位置；隨后判斷走位標志是否置位，若已置位則根據(jù)目標位置和當前位置進行位置規(guī)劃；若未置位則根據(jù)現(xiàn)有的位置規(guī)劃計算得到當前控制周期的位置給定值；接著根據(jù)位置給定值和反饋值計算得到控制輸出量DA 值，通過SPI 接口把DA 值寫入到DAC 模塊中；最后通過CAN 把當前運動狀態(tài)上報至主控制器，退出中斷函數(shù)。

圖8 從控制器軟件中斷函數(shù)流程圖

這樣，通過主控制軟件和從控制軟件設計，系統(tǒng)在CAN 通信和同步脈沖的作用下，各個從控制器驅(qū)動運動缸同步的逐漸運動到目標位置，完成平臺的姿態(tài)調(diào)整。

4 穩(wěn)定平臺姿態(tài)反解算法

六自由度平臺作為船用穩(wěn)定平臺下臺面與船甲板固定，當船舶發(fā)生波浪搖擺運動時，下臺面隨著船舶發(fā)生波浪運動，通過調(diào)整六個運動缸的長度使得上平臺保持水平狀態(tài)。因此，在進行姿態(tài)反解計算時，取下臺面姿態(tài)為船舶甲板姿態(tài)，設定上臺面姿態(tài)為水平姿態(tài)，通過這兩個臺面姿態(tài)計算得到六個運動缸長度。

穩(wěn)定平臺姿態(tài)反解算法：分別以上下平臺中心點Ot 和Ob 建立直角坐標系，如圖9 所示。

圖9 六自由度平臺坐標系

圖10 為下平臺（左）和上平臺（右）的平面坐標，圖中：b1～b6表示下平臺六個虎克鉸，t1～t6表示上平臺六個虎克鉸，Rb和Rt表示下平臺和上平臺半徑，rb、rt表示下、上平臺兩相鄰近虎克鉸間一半夾角，Sb、St表示下、上平臺兩相鄰近虎克鉸一半間距。

根據(jù)平臺半徑、半角可以計算得到上下平臺各個虎克鉸的坐標，把各虎克鉸的平面坐標加上Z 軸縱坐標，即可得到b1～b6、t1～t6在圖9 中的三維坐標，其中b1～b6的縱坐標為0，t1～t6的縱坐標為平臺處于中立位時的高度。由此可得出每個運動缸的向量值：

對向量取模，可以計算得到各運動缸的初始長度：

當船舶發(fā)生波浪運動時，由于下平臺是與船甲板相連，因此下平臺6 個虎克鉸的位置坐標均會發(fā)生變化，由bn移動到，波浪運動后的下平臺虎克鉸位置為：

式中：R（a，φ，ψ）為旋轉(zhuǎn)矩陣；a、φ、ψ分別為船舶的橫搖角、縱搖角和艏搖角；c為平移向量。

由此可以計算出波浪運動后的各運動缸的向量為：

對向量取模，可以計算得到各運動缸的實際長度為：

通過式（2）和式（5），可以計算得到各運動缸的伸長/收縮量為：

至此，完成穩(wěn)定平臺波浪補償?shù)淖藨B(tài)反解。

5 試驗結(jié)果與分析

控制系統(tǒng)在完成設計和調(diào)試后進行了平臺控制，性能良好；隨后進行了相關驗證實驗，包括：定幅值和頻率的橫搖和縱搖正弦運動，擺幅角為13°，頻率為0.2 Hz。試驗時利用上位機采集各運動缸位置和速度信息，觀察六個運動缸的運動情況。

試驗結(jié)構(gòu)表明：6 個運動缸的同步性能較好。由此說明該控制系統(tǒng)具有較高的伺服性能，系統(tǒng)實時同步性較高，能夠滿足船用六自由度穩(wěn)定平臺的控制要求。

6 結(jié)論

本文設計了一種基于CAN 總線的分布式船用六自由度穩(wěn)定平臺同步控制系統(tǒng)。該系統(tǒng)包括一個主控制器、六個分控制器、CAN 總線、同步脈沖信號：主、分控制器之間通過CAN 總線通信，系統(tǒng)設置了同步信號以保證伺服系統(tǒng)的實時性和同步性。

本文基于CAN 總線的分布式六自由度運動平臺同步控制系統(tǒng)設計，分別從硬件和軟件方面介紹了控制系統(tǒng)的設計思路，介紹了穩(wěn)定平臺姿態(tài)反解算法，最后通過試驗對控制系統(tǒng)性能進行驗證。試驗結(jié)果表明，控制系統(tǒng)具有較高的伺服性能，系統(tǒng)實時同步性較高，能夠滿足六自由度平臺控制要求。