基于嵌入式系統(tǒng)和CAN總線的檢修機械臂控制系統(tǒng)

                        
    系統(tǒng)主要由分布在三個區(qū)域的裝置組成。(1)位于核反應(yīng)堆廠房外面的集裝箱內(nèi)的配電系統(tǒng)和監(jiān)控系統(tǒng)；(2)位于蒸汽發(fā)生器附近的SMC控制器，最大距離可達(dá)到100m；(3)位于蒸汽發(fā)生器下封頭附近的機械臂本體、檢修工具、監(jiān)控攝像頭，其中集裝箱與SMC控制器最大距離可達(dá)到150m。
    下面介紹三部分之間的連接關(guān)系及主要功能。
    (1)集裝箱與SMC控制器：①由集裝箱內(nèi)配電系統(tǒng)向SMC控制器提供220VAC、48VAC等電源輸出；②集裝箱內(nèi)監(jiān)控系統(tǒng)與SMC控制器采用現(xiàn)場總線通訊；③集裝箱內(nèi)監(jiān)控系統(tǒng)向SMC控制器提供遠(yuǎn)程復(fù)位（Reset）信號；④SMC控制器向集裝箱內(nèi)監(jiān)控系統(tǒng)提供自身工作狀態(tài)信號。
    (2)SMC控制器與機械臂本體：①SMC控制器向機械臂本體各直流伺服電機分別提供PWM直流伺服驅(qū)動信號；②SMC控制器向機械臂本體各旋轉(zhuǎn)變壓器提供中頻勵磁信號；③SMC控制器向機械臂本體安裝偏差檢測電位器提供激勵信號；④R旋轉(zhuǎn)變壓器向SMC控制器提供角度反饋信號；⑤偏差檢測電位器向SMC控制器提供偏差檢測信號。
    (3)集裝箱與機械臂本體：①由集裝箱內(nèi)配電系統(tǒng)向機械臂本體供應(yīng)檢修工具電源、監(jiān)控攝像頭電源、散熱裝置電源；②SG附近監(jiān)控攝像頭向集裝箱內(nèi)監(jiān)控系統(tǒng)提供監(jiān)控視頻。
    由于機械臂控制算法計算量大、控制復(fù)雜，所以采用上、下位機的分布式控制方法來設(shè)計整個控制系統(tǒng)。
2 硬件電路設(shè)計
    機械臂控制系統(tǒng)有三個主要部件，即工控機、SMC控制器和機械臂本體。工控機主要負(fù)責(zé)機械臂控制算法、正反解算法、工作狀態(tài)顯示、數(shù)據(jù)記錄以及實時監(jiān)控，保持與SMC控制器中六個關(guān)節(jié)控制板的實時通訊；SMC控制器是機械臂的直接控制部件，它接收工控機通過CAN通訊傳過來的機械臂角度、速度及加速度信息，對機械臂本體進(jìn)行相應(yīng)的控制，同時將機械臂的關(guān)節(jié)角度及運行狀態(tài)參數(shù)通過CAN通訊傳給工控機，機械臂本體接收SMC控制器的指令，并按照指令要求做出機械動作，并將角度信號反饋給SMC控制器。
2.1 ＳＭＣ控制器硬件電路設(shè)計
    系統(tǒng)的硬件框圖如圖2所示。系統(tǒng)處理器采用TI公司的TMS320LF2407,它是TI公司新推出的高性能16位數(shù)字信號處理器，是24X家族中的新成員，專門為電極控制與運動控制數(shù)字化實現(xiàn)而設(shè)計。完全履行CAN2.0規(guī)范，支持11位標(biāo)準(zhǔn)和29位擴展標(biāo)識符，完全能滿足系統(tǒng)對電機控制與CAN通訊功能。

                  
    (1)為了反饋關(guān)節(jié)電機的位置,提高控制精度,使系統(tǒng)運行更加平穩(wěn)，在電機軸上和減速器后分別安裝了一只位置檢測元件旋轉(zhuǎn)變壓器，用旋轉(zhuǎn)變壓器-數(shù)字轉(zhuǎn)換器（RDC）進(jìn)行軸角解碼，將關(guān)節(jié)的位置轉(zhuǎn)換成21位的自然二進(jìn)制碼送給DSP，實現(xiàn)位置反饋。
    (2)電機選取瑞士MAXON公司的RE系列石墨電刷直流電機，額定電壓為24V，電源由一個24V的開關(guān)電源提供。電機驅(qū)動橋采用SA60脈沖調(diào)制型的運算放大器，原理圖如圖3所示。它能給負(fù)載提供10A的連續(xù)電流，全橋運放可在較寬的電源電壓范圍內(nèi)工作。

                        
    (3)關(guān)節(jié)電機電流檢測采用電流霍爾器件，經(jīng)過運算放大器輸入到ＤＳＰ的Ａ/Ｄ轉(zhuǎn)換中，用于檢測機械臂關(guān)節(jié)電機是否過流，由此判斷關(guān)節(jié)電機是否堵轉(zhuǎn)。
2.2 通訊接口設(shè)計
    ＣＡＮ總線是一種串行數(shù)據(jù)通訊協(xié)議，通訊速率可達(dá)１Mb/s，采用光導(dǎo)纖維作為通訊介質(zhì)，保證了通訊的速率與可靠性。數(shù)據(jù)段長度最多為8個字節(jié)，可滿足一般系統(tǒng)控制命令、工作狀態(tài)及檢測數(shù)據(jù)通訊的要求。同時8個字節(jié)不會占用總線時間過長、保證了通訊的可靠性。根據(jù)整個機械臂控制系統(tǒng)的特點，把工控機設(shè)為主控節(jié)點，負(fù)責(zé)與機械臂關(guān)節(jié)控制節(jié)點之間的通訊，對關(guān)節(jié)控制節(jié)點進(jìn)行統(tǒng)一管理。
    工控機與機械臂關(guān)節(jié)控制節(jié)點之間采用問答式的通訊方式，工控機將計算的各關(guān)節(jié)角度傳給關(guān)節(jié)控制板，之后控制板將機械臂各關(guān)節(jié)的角度信息傳給工控機，數(shù)據(jù)交換完畢后控制板根據(jù)收到的角度信息控制機械臂。工控機與關(guān)節(jié)控制板之間每隔0.1s交換一次數(shù)據(jù)，工控機每次都根據(jù)收到的角度信息進(jìn)行正反解計算。
    (1)CAN總線驅(qū)動器采用PCA82C250，它是協(xié)議控制器和物理總線之間的接口，該器件對總線提供差動發(fā)送能力并對CAN控制器提供差動額接收能力，原理圖如圖4所示。CAN總線與DSP接口電路圖如圖5所示。

                          
    (2)工控機與DSP板的CAN通訊采用CAN接口卡ＰＣＩ－５１１０，其作用是給計算機增加CAN-Bus現(xiàn)場總線接口功能。
3 軟件系統(tǒng)設(shè)計
    機械臂控制系統(tǒng)的軟件設(shè)計包括兩部分：下位機ＳＭＣ控制器的控制軟件設(shè)計和上位機監(jiān)控軟件設(shè)計。
3.1 下位機軟件設(shè)計
    μC/OS-II是專門為微處理器設(shè)計的一種搶占式實時多任務(wù)操作系統(tǒng)，與傳統(tǒng)的ＤＳＰ系統(tǒng)開發(fā)相比，其利用多任務(wù)管理、任務(wù)間同步與通信等特點，可以在一定程度上提高系統(tǒng)的可靠性和實時性，滿足機械臂控制系統(tǒng)對位置信息采集、ＣＡＮ通訊以及電流采集的實時性要求。
    軟件以TMS320LF2407的Ｔ０定時器作為操作系統(tǒng)的時鐘中斷源，中斷周期為1ms。創(chuàng)建位置環(huán)任務(wù)、速度環(huán)任務(wù)、Ａ/Ｄ轉(zhuǎn)換任務(wù)、ＣＡＮ通訊任務(wù)和角度采集，根據(jù)機械臂系統(tǒng)的工作特性和功能要求，系統(tǒng)任務(wù)劃分如圖６所示。系統(tǒng)任務(wù)間的通信和同步用到的系統(tǒng)服務(wù)有消息郵箱和信號量。信號量用于控制共享資源的使用權(quán)及激發(fā)其他任務(wù)的產(chǎn)生，消息郵箱用于通知任務(wù)的產(chǎn)生；本系統(tǒng)中的任務(wù)分為三種:定時產(chǎn)生的任務(wù)，由其他任務(wù)激活而不需要等待的任務(wù)，由中斷觸發(fā)產(chǎn)生的任務(wù)。圖６同時也反映本系統(tǒng)任務(wù)調(diào)度情況。

                        
3.2 機械臂控制器設(shè)計
    機械臂控制原理框圖如圖7所示。

                      
    控制器由串聯(lián)的三個環(huán)構(gòu)成，其中包括兩個模擬內(nèi)環(huán)（電流環(huán)，速度環(huán)）和一個控制環(huán)。
    控制算法綜合采用積分PID控制算法和帶死區(qū)PID控制算法：積分分離PID控制算法既能保證系統(tǒng)有較快的響應(yīng)速度，又有較高的控制精度；帶死區(qū)PID控制可使機械裝置運動到位后，控制作用不會頻繁變動，避免不必要的機械磨損。這兩種控制算法魯棒性好、可靠性高、算法簡單，能滿足系統(tǒng)的控制要求。
3.3 系統(tǒng)main函數(shù)結(jié)構(gòu)框架
    在主函數(shù)中用OSTaskCreate( )函數(shù)建立初始化任務(wù)Task Init( )，初始化任務(wù)中建立一系列的信號量和郵箱，喚醒時鐘中斷任務(wù)。建立置環(huán)任務(wù)、速度環(huán)任務(wù)、電流環(huán)任務(wù)、Ａ/Ｄ轉(zhuǎn)換任務(wù)、ＣＡＮ通訊任務(wù)和角度采集等任務(wù)。在μC/OS-II實時內(nèi)核下整個程序的結(jié)構(gòu)框架如下：
    Viod main(void)             //主函數(shù)
    {
         硬件初始化；
         OSInit()；             //內(nèi)核的初始化
         調(diào)用OSTaskCreate( )創(chuàng)建初始化任務(wù)Task Init( )；
          OSStart( )；          //開始多任務(wù)調(diào)度
    }
    Viod TaskInit(void * data)  //任務(wù)初始化
    {
        硬件時鐘初始化；
        創(chuàng)建用戶任務(wù)；
        定時檢查系統(tǒng)的狀態(tài)；
        定時復(fù)位看門狗；
    }
    ISR//中斷服務(wù)程序
    {
       保存處理器寄存器的值；
       OSIntEnter( )；
       執(zhí)行中斷函數(shù)；
       OSIntExit( )；
       恢復(fù)處理器寄存器的值；
       中斷返回；
    }
3.4 工控機監(jiān)控軟件設(shè)計
    監(jiān)控軟件功能模式分為安裝、工作、調(diào)試模式。工作、安裝模式分為實和虛兩種模式。工作過程是指機械臂腳趾已經(jīng)插到管板孔中并脹緊、機械臂處于倒掛狀態(tài)后，完成堵管和其他檢測的工作過程。安裝過程是指機械臂進(jìn)入人孔，其四個腳趾安裝到管板孔并脹緊的過程。虛模式是計算機三維仿真，不控制實際的機械臂。實模式給控制器發(fā)各種數(shù)據(jù)和信息，通過控制器對機械臂的運動和操作進(jìn)行控制，實際機械臂的位置和姿態(tài)與三維仿真保持一致。
    監(jiān)控軟件按功能劃分為五大模塊：操作界面設(shè)計、軌跡規(guī)劃與運動控制設(shè)計、任務(wù)管理模塊設(shè)計、三維運動仿真模塊設(shè)計和系統(tǒng)維護(hù)與故障診斷模塊設(shè)計。軟件功能結(jié)構(gòu)如圖8所示。

                   
    為滿足系統(tǒng)的控制與仿真功能，采用基于單文檔的OpenGL應(yīng)用程序?qū)崿F(xiàn)該監(jiān)控軟件。
 監(jiān)控軟件的主要功能如下：
    (1)主功能菜單：包括模式選擇、作業(yè)方式選擇、安裝工作、檢修工作選擇等。
    (2)參數(shù)設(shè)置：CAN通訊設(shè)置、機械臂PID控制參數(shù)設(shè)置等。
    (3)機械臂信息顯示：各關(guān)節(jié)位置姿態(tài)信息、CAN通訊狀態(tài)、傳感器工作狀態(tài)、關(guān)節(jié)控制電機電流等。
    (4)機械臂控制：JOG單關(guān)節(jié)控制、JOY六關(guān)節(jié)聯(lián)動控制、自動運行控制、關(guān)節(jié)封鎖控制等。
    (5)記錄模式及數(shù)據(jù)分析：檢修路徑選擇和記錄，路徑優(yōu)化等。
    本文以實際工程實踐中的蒸汽發(fā)生器六自由度檢修機械臂為研究對象，通過大量的理論研究和實際調(diào)試試驗，設(shè)計了一套完整的機械臂控制系統(tǒng)。整套系統(tǒng)的控制精度、可靠性及動/靜態(tài)特性等均能達(dá)到技術(shù)要求，可以很好地完成技術(shù)要求中的各項功能，并應(yīng)用到實際工作中。