基于嵌入式系統和CAN總線(xiàn)的檢修機械臂控制系統

　　蒸汽發(fā)生器(Steam Generator)是核動(dòng)力裝置中一、二回路之間的連接樞紐，也是核動(dòng)力裝置運行中發(fā)生故障最多的設備之一。蒸汽發(fā)生器的故障大多是由于各種腐蝕使傳熱管或管與管板接頭處發(fā)生泄漏，從而影響動(dòng)力裝置的安全運行。由于蒸汽發(fā)生器中存在放射性物質(zhì)，當發(fā)生泄漏時(shí)，人不能進(jìn)入其中進(jìn)行維修，這時(shí)就需要機械臂代替人完成維修工作。機械臂檢修系統即通過(guò)六自由度機械臂對蒸汽發(fā)生器內部進(jìn)行檢修和維護。該機械臂有六個(gè)關(guān)節，六軸聯(lián)動(dòng)；主要用于蒸汽發(fā)生器（SG）一回路側檢修活動(dòng)。機械臂的手端裝有適配器，可以攜掛不同的檢修工具，即可根據需要對蒸汽發(fā)生器實(shí)施不同程度的維護。

　　由于該機械臂的工作環(huán)境的特殊性，所以對機械臂整個(gè)控制系統的可靠性提出了更高的要求，這就需要良好的硬件電路設計、功能更強大的微處理器和更好的軟件實(shí)現方法。本文以蒸汽發(fā)生器六自由度檢修機械臂為應用背景，給出六自由度檢修機械臂的硬件體系結構和上下位機軟件設計方法。在硬件上采用TI公司的TMS320LF2407處理器作為主控制芯片組成SMC控制器，采用工控機作為監控計算機，SMC控制器與監控計算機用CAN總線(xiàn)連接。監控計算機主要完成機械臂控制算法、數據記錄以及實(shí)時(shí)監控，SMC控制器實(shí)現機械臂的控制、傳感器數據采集與通訊功能。在軟件上，SMC控制器采用μC/OS-II嵌入式實(shí)時(shí)操作系統，并采用模塊化程序設計方法，方便軟件系統管理和后續升級，在監控計算機上采用Visual C++與OpenGL設計監控軟件。

1 檢修機械臂系統描述

　　機械臂控制系統的組成框圖如圖1所示。

　　系統主要由分布在三個(gè)區域的裝置組成。(1)位于核反應堆廠(chǎng)房外面的集裝箱內的配電系統和監控系統；(2)位于蒸汽發(fā)生器附近的SMC控制器，最大距離可達到100m；(3)位于蒸汽發(fā)生器下封頭附近的機械臂本體、檢修工具、監控攝像頭，其中集裝箱與SMC控制器最大距離可達到150m。

　　下面介紹三部分之間的連接關(guān)系及主要功能。
　　
　　(1)集裝箱與SMC控制器：①由集裝箱內配電系統向SMC控制器提供220VAC、48VAC等電源輸出；②集裝箱內監控系統與SMC控制器采用現場(chǎng)總線(xiàn)通訊；③集裝箱內監控系統向SMC控制器提供遠程復位（Reset）信號；④SMC控制器向集裝箱內監控系統提供自身工作狀態(tài)信號。

　　(2)SMC控制器與機械臂本體：①SMC控制器向機械臂本體各直流伺服電機分別提供PWM直流伺服驅動(dòng)信號；②SMC控制器向機械臂本體各旋轉變壓器提供中頻勵磁信號；③SMC控制器向機械臂本體安裝偏差檢測電位器提供激勵信號；④R旋轉變壓器向SMC控制器提供角度反饋信號；⑤偏差檢測電位器向SMC控制器提供偏差檢測信號。

　　(3)集裝箱與機械臂本體：①由集裝箱內配電系統向機械臂本體供應檢修工具電源、監控攝像頭電源、散熱裝置電源；②SG附近監控攝像頭向集裝箱內監控系統提供監控視頻。

　　由于機械臂控制算法計算量大、控制復雜，所以采用上、下位機的分布式控制方法來(lái)設計整個(gè)控制系統。

2 硬件電路設計

　　機械臂控制系統有三個(gè)主要部件，即工控機、SMC控制器和機械臂本體。工控機主要負責機械臂控制算法、正反解算法、工作狀態(tài)顯示、數據記錄以及實(shí)時(shí)監控，保持與SMC控制器中六個(gè)關(guān)節控制板的實(shí)時(shí)通訊；SMC控制器是機械臂的直接控制部件，它接收工控機通過(guò)CAN通訊傳過(guò)來(lái)的機械臂角度、速度及加速度信息，對機械臂本體進(jìn)行相應的控制，同時(shí)將機械臂的關(guān)節角度及運行狀態(tài)參數通過(guò)CAN通訊傳給工控機，機械臂本體接收SMC控制器的指令，并按照指令要求做出機械動(dòng)作，并將角度信號反饋給SMC控制器。

　　2.1 ＳＭＣ控制器硬件電路設計

　　系統的硬件框圖如圖2所示。系統處理器采用TI公司的TMS320LF2407,它是TI公司新推出的高性能16位數字信號處理器，是24X家族中的新成員，專(zhuān)門(mén)為電極控制與運動(dòng)控制數字化實(shí)現而設計。完全履行CAN2.0規范，支持11位標準和29位擴展標識符，完全能滿(mǎn)足系統對電機控制與CAN通訊功能。

　　(1)為了反饋關(guān)節電機的位置,提高控制精度,使系統運行更加平穩，在電機軸上和減速器后分別安裝了一只位置檢測元件旋轉變壓器，用旋轉變壓器-數字轉換器（RDC）進(jìn)行軸角解碼，將關(guān)節的位置轉換成21位的自然二進(jìn)制碼送給DSP，實(shí)現位置反饋。

　　(2)電機選取瑞士MAXON公司的RE系列石墨電刷直流電機，額定電壓為24V，電源由一個(gè)24V的開(kāi)關(guān)電源提供。電機驅動(dòng)橋采用SA60脈沖調制型的運算放大器，原理圖如圖3所示。它能給負載提供10A的連續電流，全橋運放可在較寬的電源電壓范圍內工作。

　　(3)關(guān)節電機電流檢測采用電流霍爾器件，經(jīng)過(guò)運算放大器輸入到ＤＳＰ的Ａ/Ｄ轉換中，用于檢測機械臂關(guān)節電機是否過(guò)流，由此判斷關(guān)節電機是否堵轉。

　　2.2 通訊接口設計

　?。茫粒慰偩€(xiàn)是一種串行數據通訊協(xié)議，通訊速率可達１Mb/s，采用光導纖維作為通訊介質(zhì)，保證了通訊的速率與可靠性。數據段長(cháng)度最多為8個(gè)字節，可滿(mǎn)足一般系統控制命令、工作狀態(tài)及檢測數據通訊的要求。同時(shí)8個(gè)字節不會(huì )占用總線(xiàn)時(shí)間過(guò)長(cháng)、保證了通訊的可靠性。根據整個(gè)機械臂控制系統的特點(diǎn)，把工控機設為主控節點(diǎn)，負責與機械臂關(guān)節控制節點(diǎn)之間的通訊，對關(guān)節控制節點(diǎn)進(jìn)行統一管理。

　　工控機與機械臂關(guān)節控制節點(diǎn)之間采用問(wèn)答式的通訊方式，工控機將計算的各關(guān)節角度傳給關(guān)節控制板，之后控制板將機械臂各關(guān)節的角度信息傳給工控機，數據交換完畢后控制板根據收到的角度信息控制機械臂。工控機與關(guān)節控制板之間每隔0.1s交換一次數據，工控機每次都根據收到的角度信息進(jìn)行正反解計算。

　　(1)CAN總線(xiàn)驅動(dòng)器采用PCA82C250，它是協(xié)議控制器和物理總線(xiàn)之間的接口，該器件對總線(xiàn)提供差動(dòng)發(fā)送能力并對CAN控制器提供差動(dòng)額接收能力，原理圖如圖4所示。CAN總線(xiàn)與DSP接口電路圖如圖5所示。

　　(2)工控機與DSP板的CAN通訊采用CAN接口卡ＰＣＩ－５１１０，其作用是給計算機增加CAN-Bus現場(chǎng)總線(xiàn)接口功能。

3 軟件系統設計

　　機械臂控制系統的軟件設計包括兩部分：下位機ＳＭＣ控制器的控制軟件設計和上位機監控軟件設計。

　　3.1 下位機軟件設計

　　μC/OS-II是專(zhuān)門(mén)為微處理器設計的一種搶占式實(shí)時(shí)多任務(wù)操作系統，與傳統的ＤＳＰ系統開(kāi)發(fā)相比，其利用多任務(wù)管理、任務(wù)間同步與通信等特點(diǎn)，可以在一定程度上提高系統的可靠性和實(shí)時(shí)性，滿(mǎn)足機械臂控制系統對位置信息采集、ＣＡＮ通訊以及電流采集的實(shí)時(shí)性要求。

　　軟件以TMS320LF2407的Ｔ０定時(shí)器作為操作系統的時(shí)鐘中斷源，中斷周期為1ms。創(chuàng )建位置環(huán)任務(wù)、速度環(huán)任務(wù)、Ａ/Ｄ轉換任務(wù)、ＣＡＮ通訊任務(wù)和角度采集，根據機械臂系統的工作特性和功能要求，系統任務(wù)劃分如圖６所示。系統任務(wù)間的通信和同步用到的系統服務(wù)有消息郵箱和信號量。信號量用于控制共享資源的使用權及激發(fā)其他任務(wù)的產(chǎn)生，消息郵箱用于通知任務(wù)的產(chǎn)生；本系統中的任務(wù)分為三種:定時(shí)產(chǎn)生的任務(wù)，由其他任務(wù)激活而不需要等待的任務(wù)，由中斷觸發(fā)產(chǎn)生的任務(wù)。圖６同時(shí)也反映本系統任務(wù)調度情況。

　　3.2 機械臂控制器設計

　　機械臂控制原理框圖如圖7所示。

　　控制器由串聯(lián)的三個(gè)環(huán)構成，其中包括兩個(gè)模擬內環(huán)（電流環(huán)，速度環(huán)）和一個(gè)控制環(huán)。

　　控制算法綜合采用積分PID控制算法和帶死區PID控制算法：積分分離PID控制算法既能保證系統有較快的響應速度，又有較高的控制精度；帶死區PID控制可使機械裝置運動(dòng)到位后，控制作用不會(huì )頻繁變動(dòng)，避免不必要的機械磨損。這兩種控制算法魯棒性好、可靠性高、算法簡(jiǎn)單，能滿(mǎn)足系統的控制要求。

　　3.3 系統main函數結構框架

　　在主函數中用OSTaskCreate( )函數建立初始化任務(wù)Task Init( )，初始化任務(wù)中建立一系列的信號量和郵箱，喚醒時(shí)鐘中斷任務(wù)。建立置環(huán)任務(wù)、速度環(huán)任務(wù)、電流環(huán)任務(wù)、Ａ/Ｄ轉換任務(wù)、ＣＡＮ通訊任務(wù)和角度采集等任務(wù)。在μC/OS-II實(shí)時(shí)內核下整個(gè)程序的結構框架如下：

    Viod main(void)             //主函數
    {
         硬件初始化；
         OSInit()；             //內核的初始化
         調用OSTaskCreate( )創(chuàng )建初始化任務(wù)Task Init( )；
          OSStart( )；          //開(kāi)始多任務(wù)調度
    }
    Viod TaskInit(void * data)  //任務(wù)初始化
    {
        硬件時(shí)鐘初始化；
        創(chuàng )建用戶(hù)任務(wù)；
        定時(shí)檢查系統的狀態(tài)；
        定時(shí)復位看門(mén)狗；
    }
    ISR//中斷服務(wù)程序
    {
       保存處理器寄存器的值；
       OSIntEnter( )；
       執行中斷函數；
       OSIntExit( )；
       恢復處理器寄存器的值；
       中斷返回；
    }

　　3.4 工控機監控軟件設計

　　監控軟件功能模式分為安裝、工作、調試模式。工作、安裝模式分為實(shí)和虛兩種模式。工作過(guò)程是指機械臂腳趾已經(jīng)插到管板孔中并脹緊、機械臂處于倒掛狀態(tài)后，完成堵管和其他檢測的工作過(guò)程。安裝過(guò)程是指機械臂進(jìn)入人孔，其四個(gè)腳趾安裝到管板孔并脹緊的過(guò)程。虛模式是計算機三維仿真，不控制實(shí)際的機械臂。實(shí)模式給控制器發(fā)各種數據和信息，通過(guò)控制器對機械臂的運動(dòng)和操作進(jìn)行控制，實(shí)際機械臂的位置和姿態(tài)與三維仿真保持一致。

　　監控軟件按功能劃分為五大模塊：操作界面設計、軌跡規劃與運動(dòng)控制設計、任務(wù)管理模塊設計、三維運動(dòng)仿真模塊設計和系統維護與故障診斷模塊設計。軟件功能結構如圖8所示。

　　為滿(mǎn)足系統的控制與仿真功能，采用基于單文檔的OpenGL應用程序實(shí)現該監控軟件。

　　監控軟件的主要功能如下：

　　(1)主功能菜單：包括模式選擇、作業(yè)方式選擇、安裝工作、檢修工作選擇等。

　　(2)參數設置：CAN通訊設置、機械臂PID控制參數設置等。

　　(3)機械臂信息顯示：各關(guān)節位置姿態(tài)信息、CAN通訊狀態(tài)、傳感器工作狀態(tài)、關(guān)節控制電機電流等。

　　(4)機械臂控制：JOG單關(guān)節控制、JOY六關(guān)節聯(lián)動(dòng)控制、自動(dòng)運行控制、關(guān)節封鎖控制等。

　　(5)記錄模式及數據分析：檢修路徑選擇和記錄，路徑優(yōu)化等。

　　本文以實(shí)際工程實(shí)踐中蒸汽發(fā)生器的六自由度檢修機械臂為研究對象，通過(guò)大量的理論研究和實(shí)際調試試驗，設計了一套完整的機械臂控制系統。整套系統的控制精度、可靠性及動(dòng)/靜態(tài)特性等均能達到技術(shù)要求，可以很好地完成技術(shù)要求中的各項功能，并應用到實(shí)際工作中。