基于狀態(tài)機(jī)的巡線機(jī)器人控制系統(tǒng)設(shè)計(jì)

1. 引言

1.1 巡線機(jī)器人

　　高壓輸電線路作為電力輸送的主要方式，是國(guó)民經(jīng)濟(jì)的大動(dòng)脈，其安全可靠的運(yùn)行是社會(huì)生產(chǎn)和人民生活的重要保障。由于高壓輸電線路特殊的工作環(huán)境，輸電線路上的各種設(shè)施容易損壞，因而定期進(jìn)行高壓輸電線路的巡檢是保證可靠的電力輸送的重要基礎(chǔ)。當(dāng)前，輸電線路的巡檢方式以人工為主。但是由于輸電線路多架設(shè)在崇山峻嶺之間，加之野外環(huán)境比較惡劣，采用人工巡檢的方式受到諸多的限制，這在很大程度上影響了對(duì)輸電線路的及時(shí)維護(hù)，給電能的可靠輸送埋下了安全隱患。機(jī)器人技術(shù)的發(fā)展為輸電線路的自動(dòng)巡檢帶來(lái)了可能。巡線機(jī)器人就是一類(lèi)專(zhuān)門(mén)應(yīng)用于輸電線路巡檢的機(jī)器人。輸電線路巡檢的主要任務(wù)是檢查桿塔、導(dǎo)線、避雷線、絕緣子和金具的狀態(tài)。這要求機(jī)器人能夠在輸電線路上自主的運(yùn)行，能夠自主的識(shí)別并跨越障礙物。為了滿足越障行為對(duì)機(jī)器人機(jī)械結(jié)構(gòu)的要求，同時(shí)為了最大程度上適應(yīng)輸電線路這種柔性的環(huán)境，巡線機(jī)器人被設(shè)計(jì)成兩臂懸掛式機(jī)器人采用兩個(gè)具有夾持功能的機(jī)械手臂作為行走機(jī)構(gòu)。正常巡檢時(shí)，依靠安裝在兩個(gè)手臂上的行走電機(jī)驅(qū)動(dòng)機(jī)器人，當(dāng)需要越障時(shí)，其中一只手臂上的卡爪夾住輸電線路，由機(jī)械臂的回轉(zhuǎn)機(jī)構(gòu)驅(qū)動(dòng)機(jī)器人轉(zhuǎn)身繞過(guò)障礙物。當(dāng)一臂繞過(guò)障礙物后，根據(jù)視覺(jué)傳感器提供的機(jī)械手和輸電線的相對(duì)位置，通過(guò)安裝在手臂上的擺臂機(jī)構(gòu)實(shí)現(xiàn)夾手和輸電線路的精確對(duì)準(zhǔn)。不難發(fā)現(xiàn)，兩臂懸掛式機(jī)器人在越障過(guò)程中主要需要解決的問(wèn)題是穩(wěn)定性和抓線的定位問(wèn)題。為了實(shí)現(xiàn)可靠的越障，巡線機(jī)器人機(jī)械手臂被設(shè)計(jì)成雙關(guān)節(jié)形式：機(jī)械臂底部有一個(gè)旋轉(zhuǎn)關(guān)節(jié)，可帶動(dòng)機(jī)器人做180度旋轉(zhuǎn)，機(jī)械手中間位置有一個(gè)擺動(dòng)關(guān)節(jié)，可帶動(dòng)機(jī)械手做向左向右的擺動(dòng)。之所以設(shè)計(jì)成上述結(jié)構(gòu)，主要是方便機(jī)械手脫線和抓線時(shí)的姿態(tài)調(diào)整。

1.2 巡線機(jī)器人運(yùn)動(dòng)控制系統(tǒng)

　　從上面的敘述可以看出，兩臂懸掛式巡線機(jī)器人越障行為過(guò)程比較復(fù)雜，這給運(yùn)動(dòng)控制系統(tǒng)的設(shè)計(jì)帶來(lái)了極大的困難。巡線機(jī)器人的運(yùn)動(dòng)控制系統(tǒng)主要完成兩大任務(wù)：第一，控制電機(jī)完成相應(yīng)的運(yùn)動(dòng)過(guò)程，達(dá)到需要的動(dòng)態(tài)特性和穩(wěn)定性要求。第二，運(yùn)動(dòng)過(guò)程中的保護(hù)及定位。本文主要討論運(yùn)動(dòng)控制系統(tǒng)的保護(hù)及定位功能的設(shè)計(jì)與實(shí)現(xiàn)。通常而言，運(yùn)動(dòng)控制系統(tǒng)的上面兩個(gè)功能主要有兩種實(shí)現(xiàn)方法：基于軟件的實(shí)現(xiàn)和基于硬件的實(shí)現(xiàn)。基于軟件的實(shí)現(xiàn)方法靈活性高，可以實(shí)現(xiàn)較為復(fù)雜的算法，因而，運(yùn)動(dòng)控制系統(tǒng)的第一個(gè)功能一般由軟件實(shí)現(xiàn)。但是基于軟件的實(shí)現(xiàn)方法存在最大的缺點(diǎn)是可靠性不夠高，響應(yīng)速度較慢 。而巡線機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的第二個(gè)功能即各種限位保護(hù)，意外處理，過(guò)流過(guò)載保護(hù)，零位調(diào)整等都需要較高的可靠性，用軟件實(shí)現(xiàn)上述功能時(shí)，一旦軟件出現(xiàn)諸如程序跑飛，死機(jī)等故障時(shí)，必然會(huì)對(duì)驅(qū)動(dòng)電機(jī)和機(jī)器人機(jī)構(gòu)造成很大的損壞。另一方面，機(jī)器人機(jī)構(gòu)對(duì)運(yùn)動(dòng)控制系統(tǒng)的響應(yīng)速度有較高的要求，當(dāng)出現(xiàn)意外情況時(shí)，控制系統(tǒng)必須以最短的延時(shí)給出驅(qū)動(dòng)電機(jī)相應(yīng)的控制信號(hào)，以保護(hù)電機(jī)和機(jī)構(gòu)。雖然基于實(shí)時(shí)操作系統(tǒng)的軟件在實(shí)時(shí)性上有較大的提高，但是這在很大程度上取決于系統(tǒng)運(yùn)行的任務(wù)數(shù)量，當(dāng)任務(wù)數(shù)量較多時(shí)也存在響應(yīng)速度慢的問(wèn)題。與基于軟件的實(shí)現(xiàn)方法相比較，基于硬件的實(shí)現(xiàn)方法可靠性高，響應(yīng)速度快，但最大的缺點(diǎn)是靈活性不高，實(shí)現(xiàn)過(guò)程較為復(fù)雜。故不宜采用硬件去實(shí)現(xiàn)較為復(fù)雜的控制算法。

　　對(duì)運(yùn)動(dòng)控制系統(tǒng)中的第二個(gè)主要任務(wù)，比較適合采用硬件的方法實(shí)現(xiàn)。這主要是考慮到第二個(gè)功能要求較高的可靠性和響應(yīng)速度，同時(shí)不需要復(fù)雜的運(yùn)算，屬于邏輯控制功能，比較適宜于采用硬件實(shí)現(xiàn)。

1.3 運(yùn)動(dòng)控制問(wèn)題描述

　　就巡線機(jī)器人的運(yùn)動(dòng)控制系統(tǒng)中的保護(hù)及定位系統(tǒng)而言，其本質(zhì)是根據(jù)傳感器信號(hào)給出相應(yīng)的電機(jī)控制信號(hào)。也就是說(shuō)，電機(jī)保護(hù)及定位系統(tǒng)完成控制輸入（傳感器信號(hào)）到控制輸出（電機(jī)控制信號(hào)）間的映射。電機(jī)保護(hù)及定位系統(tǒng)的特點(diǎn)決定了其非常適合于采用有限狀態(tài)機(jī)進(jìn)行設(shè)計(jì)。

　　前面的敘述已經(jīng)大致描述了運(yùn)動(dòng)保護(hù)及定位系統(tǒng)需要完成的功能。具體的說(shuō)就是，當(dāng)運(yùn)動(dòng)機(jī)構(gòu)運(yùn)動(dòng)至左側(cè)和右側(cè)限位位置時(shí)，傳感器均給出相應(yīng)的限位信號(hào)，運(yùn)動(dòng)控制系統(tǒng)據(jù)此停止電機(jī)，且限制機(jī)構(gòu)在這兩個(gè)位置的運(yùn)動(dòng)。即：若機(jī)構(gòu)順時(shí)針擺動(dòng)至限位點(diǎn)，則限位后機(jī)構(gòu)只可逆時(shí)針運(yùn)動(dòng)，順時(shí)針?lè)较虻倪\(yùn)動(dòng)被限制，反之亦然。巡線機(jī)器人運(yùn)動(dòng)控制系統(tǒng)中的一個(gè)較為復(fù)雜的功能是對(duì)兩個(gè)機(jī)械臂的定位，機(jī)械臂定位要求是，無(wú)論機(jī)械手從何方靠近中間的零位位置，總需要機(jī)械手自動(dòng)停止在一個(gè)位置-此位置為機(jī)械手與機(jī)體垂直的位置。由于我們采用的位置傳感器是霍爾效應(yīng)傳感器，傳感器探頭與磁鋼的距離小于某一閾值時(shí)傳感器便給出有效信號(hào)。因此，當(dāng)磁鋼從不同方向接近傳感器探頭時(shí)，傳感器給出限位信號(hào)的位置不同。經(jīng)過(guò)實(shí)驗(yàn)驗(yàn)證，當(dāng)磁鋼進(jìn)入某一個(gè)區(qū)域時(shí)，傳感器即刻給出有效的信號(hào)，此區(qū)域?qū)挾却蠹s為1厘米。因此，倘若均以進(jìn)入該區(qū)域即認(rèn)為機(jī)械手歸零到位，顯然，當(dāng)機(jī)械手從不同方向歸零時(shí)并不能保證機(jī)械手都垂直于機(jī)體。事實(shí)上，由于傳感器安裝的位置的影響，加之整個(gè)機(jī)械手較長(zhǎng)，當(dāng)從不同方向歸零時(shí)，機(jī)械手姿態(tài)差別很大。為了保證無(wú)　　論從何方歸零，機(jī)械手總能停止在同一位置，要求控制器只能采用傳感器的同一邊沿信號(hào)作為歸零到位信號(hào)。比如：當(dāng)機(jī)械手順時(shí)針歸零時(shí)，若檢測(cè)到傳感器信號(hào)的下降沿即停止電機(jī)，認(rèn)為歸零到位;當(dāng)機(jī)械手逆時(shí)針歸零時(shí)，需檢測(cè)到傳感器信號(hào)的上升沿（也就是順時(shí)針時(shí)的下降沿，即同一邊沿信號(hào)）時(shí)，方可認(rèn)為歸零到位。

　　在我們?cè)O(shè)計(jì)的運(yùn)動(dòng)控制系統(tǒng)中，對(duì)任一電機(jī)，控制系統(tǒng)僅給出三個(gè)控制信號(hào)：兩個(gè)方向信號(hào)和速度信號(hào)（dira,dirb和pwm），其中的兩個(gè)方向信號(hào)控制電機(jī)的轉(zhuǎn)向和開(kāi)停，dira和dirb不同時(shí)，電機(jī)方可運(yùn)動(dòng)，相同時(shí)電機(jī)停止。pwm信號(hào)也可控制電機(jī)的運(yùn)行，pwm為高電機(jī)可運(yùn)行，反之停止。機(jī)械手的定位調(diào)整系統(tǒng)需要設(shè)置三個(gè)傳感器：左限位傳感器（sensor_l），中間零位傳感器（sensor_m），右限位傳感器 （sensor_r）。對(duì)不需要?dú)w零的運(yùn)動(dòng)機(jī)構(gòu)則不設(shè)置中間零位傳感器。

2.有限狀態(tài)機(jī)設(shè)計(jì)

2.1 運(yùn)動(dòng)控制系統(tǒng)的有限狀態(tài)機(jī)設(shè)計(jì)

　　運(yùn)動(dòng)控制系統(tǒng)中的運(yùn)動(dòng)保護(hù)及定位系統(tǒng)的典型時(shí)序如下圖示：

圖1 運(yùn)動(dòng)保護(hù)及定位系統(tǒng)時(shí)序與框圖

　　從上面的是時(shí)序圖可以看出，運(yùn)動(dòng)控制系統(tǒng)的時(shí)序顯著的分為四段：第一段L1：為運(yùn)動(dòng)機(jī)構(gòu)的左限位階段。即當(dāng)運(yùn)動(dòng)至左限位點(diǎn)時(shí)，控制系統(tǒng)應(yīng)該停止電機(jī)并限制此方向運(yùn)動(dòng)。第二段L2：僅有機(jī)械手臂具有此段時(shí)序特征。當(dāng)機(jī)械手以順時(shí)針?lè)较驍[向垂直位置時(shí)，若到達(dá)垂直位置（sensor_m由高變低）則控制系統(tǒng)停止電機(jī)。但與限位不同，此時(shí)控制系統(tǒng)不可限制機(jī)構(gòu)任意方向的運(yùn)動(dòng)。為了使得機(jī)械手到達(dá)垂直位置后仍可向左或向右運(yùn)動(dòng)，在我們的設(shè)計(jì)中引入另一控制量：電機(jī)的速度信號(hào)pwm。第三段L3：與第二段相同，也為機(jī)械手特有。當(dāng)機(jī)械手以逆時(shí)針向垂直位置運(yùn)動(dòng)時(shí)，與順時(shí)針時(shí)不同，它需要在傳感器信號(hào)由低變高時(shí)方可停止電機(jī)，當(dāng)然也不能限制機(jī)構(gòu)任意方向的運(yùn)動(dòng)。第四段L4：為運(yùn)動(dòng)機(jī)構(gòu)的右限位階段，與第一段相似，當(dāng)機(jī)構(gòu)運(yùn)動(dòng)至右限位點(diǎn)時(shí)，控制系統(tǒng)需要自動(dòng)停止電機(jī)并限制此方向的運(yùn)動(dòng)。

　　根據(jù)運(yùn)動(dòng)控制系統(tǒng)的典型時(shí)序特征，可以制定出相應(yīng)的狀態(tài)和狀態(tài)轉(zhuǎn)換條件：首先，設(shè)置初始化狀態(tài)s0，系統(tǒng)上電或者復(fù)位后都自動(dòng)進(jìn)入該狀態(tài)。上電時(shí)，在此狀態(tài)對(duì)內(nèi)部信號(hào)和輸出進(jìn)初始化。此狀態(tài)也是系統(tǒng)默認(rèn)的電機(jī)停止?fàn)顟B(tài)。正常運(yùn)動(dòng)的情況下，機(jī)構(gòu)順時(shí)針和逆時(shí)針?lè)謩e對(duì)應(yīng)兩個(gè)狀態(tài)s1和s2。當(dāng)機(jī)構(gòu)順時(shí)針歸零后，與限位不同，由于此時(shí)不可限制機(jī)構(gòu)任意方向的運(yùn)動(dòng)，因而當(dāng)pwm信號(hào)拉低后狀態(tài)機(jī)需要進(jìn)入狀態(tài)s3,s3其實(shí)為一個(gè)過(guò)渡狀態(tài)，通過(guò)s3,狀態(tài)機(jī)可根據(jù)此時(shí)微處理器給出的控制信號(hào)進(jìn)入s1和s2運(yùn)行。同樣，當(dāng)機(jī)構(gòu)逆時(shí)針歸零時(shí)，與順時(shí)針歸零不同，它需要檢測(cè)到中間位置傳感器拉高后才能停止電機(jī)。所以當(dāng)逆時(shí)針運(yùn)動(dòng)時(shí)，一旦檢測(cè)到sensor_m變低，狀態(tài)機(jī)即進(jìn)入過(guò)渡狀態(tài)s5,在此狀態(tài)，若sensor_m變高，即意味著機(jī)構(gòu)已經(jīng)離開(kāi)傳感器區(qū)域，到達(dá)垂直位置，此時(shí)狀態(tài)機(jī)進(jìn)入狀態(tài)s6。從上面的分析可以看出：對(duì)于順時(shí)針和逆時(shí)針兩種不同的狀態(tài)下機(jī)構(gòu)歸零，我們分別設(shè)置了兩個(gè)特殊的過(guò)渡狀態(tài)集｛s3,s4｝和｛s5,s6｝。這主要是由于歸零操作不同于限位。歸零后，控制系統(tǒng)不能限制機(jī)構(gòu)任何方向的運(yùn)動(dòng)，因而需要一個(gè)額外的控制輸入作為歸零后狀態(tài)機(jī)狀態(tài)轉(zhuǎn)換的觸發(fā)信號(hào)。根據(jù)上面的分析，可以得到狀態(tài)機(jī)的狀態(tài)轉(zhuǎn)換圖如下：
 
圖2 有限狀態(tài)機(jī)狀態(tài)轉(zhuǎn)換圖

2.2設(shè)計(jì)仿真與實(shí)現(xiàn)

2.2.1 設(shè)計(jì)仿真

　　本設(shè)計(jì)在ISE8.2i+Modelsim XE III 6.0d平臺(tái)上綜合，布局布線和仿真。其布局布線后仿真的時(shí)序圖如下：
 
圖3 時(shí)序仿真圖

　　從上面的后仿真時(shí)序可以看出，當(dāng)sensor_l變低時(shí)，dira和dirb同時(shí)變低，只有當(dāng)方向信號(hào)dir改變時(shí)，dira和dirb才隨之改變。sensor_r的變化同sensor_l相似。仿真時(shí)序中比較關(guān)鍵的一段是sensor_m兩次變低的情況，分別代表順時(shí)針和逆時(shí)針歸零的兩種情況。從上圖可以看出，所設(shè)計(jì)的狀態(tài)機(jī)很好的實(shí)現(xiàn)了對(duì)傳感器同一邊沿信號(hào)的識(shí)別，同時(shí)在加入了控制信號(hào)pwm后，使得機(jī)構(gòu)歸零后還可以進(jìn)行任意方向的運(yùn)動(dòng)，滿足了設(shè)計(jì)要求。

2.2.2設(shè)計(jì)實(shí)現(xiàn)

　　將設(shè)計(jì)的狀態(tài)機(jī)采用VHDL語(yǔ)言編碼并在xilinx公司的CPLD器件XC9572上實(shí)現(xiàn)，經(jīng)過(guò)實(shí)驗(yàn)驗(yàn)證，所設(shè)計(jì)的狀態(tài)機(jī)是安全可靠的。

3.結(jié)論

　　機(jī)器人控制系統(tǒng)的實(shí)現(xiàn)主要有兩種方法。基于軟件的實(shí)現(xiàn)方法在各種苛刻的實(shí)際應(yīng)用中顯示出了種種不足。可編程器件的發(fā)展為控制系統(tǒng)的實(shí)現(xiàn)提供了一種全新的模式�；�于可編程器件的控制系統(tǒng)實(shí)現(xiàn)，可靠性好，實(shí)現(xiàn)簡(jiǎn)單，靈活性好，相對(duì)于傳統(tǒng)的軟件實(shí)現(xiàn)和由分離元件實(shí)現(xiàn)的控制系統(tǒng)，基于可編程器件的控制系統(tǒng)有著諸多無(wú)法替代的優(yōu)點(diǎn)。本文根據(jù)控制對(duì)象對(duì)控制器時(shí)序要求，抽象出控制系統(tǒng)的行為級(jí)描述，并據(jù)此設(shè)計(jì)了有限狀態(tài)機(jī)的狀態(tài)。并采用EDA軟件進(jìn)行了綜合，布局布線和仿真，最終在CPLD上實(shí)現(xiàn)了所設(shè)計(jì)的運(yùn)動(dòng)控制系統(tǒng)，為控制系統(tǒng)的數(shù)字化設(shè)計(jì)提供了一種行之有效的方法。

　　本文的創(chuàng)新點(diǎn)是將有限狀態(tài)機(jī)應(yīng)用于機(jī)器人控制系統(tǒng)的硬件設(shè)計(jì)，并在可編程器件上實(shí)現(xiàn)了所做設(shè)計(jì)。這是對(duì)常見(jiàn)的基于軟件的控制系統(tǒng)實(shí)現(xiàn)方法和由分離元件構(gòu)造的基于硬件的控制系統(tǒng)實(shí)現(xiàn)方法的極大改善，大大提高了控制系統(tǒng)的可靠性，抗干擾性和實(shí)時(shí)性。