管道探測蛇形機器人控制系統(tǒng)設計及仿真

邢利輝，王亞慧，鄧 蕊

(北京建筑大學電氣與信息工程學院，北京100044)

1 引 言

蛇形機器人是一種新型的柔性機器人，可用在各種探測任務中.目前燃氣管道正在日益增加，而燃氣管道往往深埋地下，經(jīng)過長期的土壤腐蝕或不可抗拒的自然災害必將老化，出現(xiàn)泄漏、爆炸等安全事故，鑒于此，我們開發(fā)的這款蛇形機器人可在各類燃氣管道中進行檢測任務.燃氣管道的直徑從200mm-500mm 不等，有的管道的直徑甚至更小.目前可以在管道中進行巡查的機器人多為輪式機器人，這種機器人只適合在較粗的管道中進行探測，而在較細管道中無法進入，并且在豎直管道中也顯的無能為力.

為克服上述技術和環(huán)境適應性的不足，我們提出了一種利用蛇形機器人檢測燃氣管道內部環(huán)境的方法.蛇形機器人因身體細長，橫截面小，靈活性強，適應環(huán)境等優(yōu)點［1，2］，可以靈活地進出各種管徑和角度的管道，當管徑較小時以直線方式探測，在較大的管道中以螺旋方式探測［3］.收集實時內部圖像和監(jiān)測數(shù)據(jù)，因此它可以很好地應用于各種管道檢測領域.該蛇形機器人可以對姿態(tài)、管道內部圖像，溫濕度等信息的獲取和處理，以達到對傳輸管道內部的檢測和維護.

2 蛇形機器人模塊化設計與建模

2.1 蛇形機器人關節(jié)設計

與以往管道外部的蛇形機器人不同，該機器人在管道內部運動對本身的關節(jié)不可見，所以行走機構有所不同.首先為了適應燃氣的圓形管道，蛇關節(jié)外部為圓形，其圓形關節(jié)外部兩端裝有兩組小車輪，均為從動輪，在模塊中添加一個電機并使用齒輪傳動，帶動后端車輪轉動，進而推動機器人前進.其從動輪主要是為了能平滑行走［4］.其關節(jié)刨面圖1 所示.

關節(jié)內部主要包括俯仰舵機、旋轉舵機、控制器和一些連接件.圖中俯仰舵機通過U 型連接件固定后準備與下一個關節(jié)相連，另一側也為一個U 型連接件，該連接件與電機相連，最終電機帶動主動輪向前或向后運動，而舵機通過旋轉角度控制整個蛇以螺旋方式前進.在不影響舵機旋轉的情況下，控制器放置在舵機與電機連接的側面，為了保證蛇關節(jié)較小，經(jīng)測控制器大小(6.24cm×6.24cm)為預留的空隙大小.經(jīng)輸出的PWM 信號控制俯仰舵機和旋轉舵機，在旋轉舵機下方是固定件，固定舵機在關節(jié)內不晃動，我們可以看到在其下方有預留位置，可以放置傳感器和一些連接器件.

圖1 蛇關節(jié)剖面圖Fig.1 Snake joint planing

2.2 ADAMS 建模及仿真

在建立ADAMS 仿真環(huán)境后，針對不同管徑的豎直管道進行了建模分析，由于我們的蛇關節(jié)是可拆卸的，在不同管徑中，使用不同多少的單元模塊即可完成在管道內部的檢測任務，最終使關節(jié)按照一定的角度螺旋前行.

和水平運動的管道中比較，在豎直管道中蛇的所有關節(jié)重量都是蛇向上攀爬的阻力，所以比水平管道有更大的豎直向上動力才能推動蛇向上爬.

圖2 250mm 管道蛇姿態(tài)圖Fig.2 250mm pipe snake attitude diagram

如圖2 是蛇形機器人在250mm 的豎直管道中，從下向上依次將關節(jié)起名為J1-J7，選取特殊的關節(jié)進行受力分析，規(guī)定蛇向上運動方向為Z 軸，垂直于顯示面為X 軸，同時垂直于X 和Z 軸方向為Y 軸.首先選取J1 進行受力分析，如圖3所示，J1 共受四個力的作用，地面對J1 的引力G1(方向沿Z軸負方向豎直向下)，管壁對J1 的支持力Fz1(方向垂直指向于接觸面凹面)和管壁對J1 的摩擦力Ff1(方向沿著蛇運動趨勢的反方向)，相鄰關節(jié) J2 對 J1 的作用力 FJ2-J1.其中每個關節(jié)共重為380g，所有受力在全局坐標系下進行受力分解得到靜力學平衡方程:

從ADAMS 中可輸出各個力在X 軸、Y 軸和Z 軸上分力的大小，其中 Ff1x=5.6N，F(xiàn)z1x=8.6N，F(xiàn)J2－J1x=－14.2N.Ff1y=15.8N，F(xiàn)z1y=－9.7N，F(xiàn)J2－J1y=－6.1N.Ff1z=－12.1N，G1=0.38* 9.8=－3.7N，F(xiàn)z1z=4.8N，F(xiàn)J2－J1z=11N，由以上輸出的力大小可以看出蛇形機器人滿足靜力學平衡方程.

圖3 關節(jié)J1 靜力學分析Fig.3 Static analysis of joint J1

通過對管道蛇形機器人的建模仿真，設置好關節(jié)的質量、關節(jié)之間的扭矩，還可以得到關節(jié)之間力矩、旋轉的相對角速度等，另外也可以得到蛇在管道內部運動的位移、速度、加速度、相對角速度、相對加速度等值.在250mm 的管道中對蛇形機器人進行仿真得到運動的速度、加速度、位移、角速度，由仿真結果圖可以看出機器人在管道內重復的以螺旋方式緩慢前行.

3 控制系統(tǒng)整體結構設計

探測機器人控制系統(tǒng)的功能是將各個執(zhí)行機構如舵機、電機等的控制以及各類傳感器采集的數(shù)據(jù)傳輸控制整合到上位機中，實現(xiàn)操作人員的遠程控制功能［5］.控制系統(tǒng)貫穿整個探測機器人的控制信號傳輸及數(shù)據(jù)采集信號傳輸［6］.根據(jù)探測機器人的特點，文章設計采用CAN 總線技術設計探測機器人的控制系統(tǒng)［7］.

在操作人員電腦端，設計使用組態(tài)王軟件編譯上位機監(jiān)控系統(tǒng)，并與機器人主控制系統(tǒng)相聯(lián)，由操作人員在上位機監(jiān)控系統(tǒng)中，發(fā)送控制指令，控制探測機器人的姿態(tài)及運動等，如控制各單元模塊中舵機的轉角，達到控制機器人姿態(tài)的目的;控制電機的轉動，達到控制器機器人的前進后退旋轉等目的;控制傳感器數(shù)據(jù)傳輸，達到采集管道內各項信息的目的等.上位機向下連接由微控制器控制的主控器單元，主控器通過CAN 總線連接多個從控制器，使探測機器人具備良好的可擴展性，每個模塊均由一個俯仰舵機和一個旋轉舵機組成，所以每個模塊中有兩個從控制器分別控制俯仰舵機和旋轉舵機，同時CAN 總線也能夠滿足探測機器人的實時性要求.探測機器人的從控制器采用嵌入式單片機技術，并將探測機器人中應用到的執(zhí)行機構，如伺服舵機，電機，以及傳感器設備搭載在從控制器上，使得每個嵌入式單片機可以獨立處理單個單元模塊的運動，提高系統(tǒng)的穩(wěn)定性和實時性.形機器人控制系統(tǒng)整體框架如圖4.

3.1 基于CAN 總線的控制方法

根據(jù)蛇形機器人的特點，文章構建了以CAN 總線為基礎的蛇形機器人控制系統(tǒng).控制系統(tǒng)的上層設計為上位機監(jiān)控系統(tǒng)［8］，通過在PC 上位機上，構建機器人控制的上位機人機交互界面，作為探測機器人的控制監(jiān)控平臺;上位機再與機器人的主控制器相聯(lián)，主控制器接收上位機發(fā)送的動作指令，并將指令通過CAN 總線系統(tǒng)，發(fā)送給掛載在總線上的從控制器.在探測機器人的從控制器上，帶載著可由微控制器發(fā)出PWM(Pulse-Width Modulation，脈寬調制 )脈沖信號控制的伺服電機及伺服舵機［9］.從控制器接收到控制探測機器人運動的指令，即對從控制器各自控制的執(zhí)行機構發(fā)送控制指令，已使機器人做出變換姿態(tài)，改變運動狀態(tài)的動作.

圖4 控制系統(tǒng)整體框架Fig.4 Control system overall framework

在蛇形機器人的蜿蜒運動控制中為例，分析它的控制方法.圖5 是蜿蜒運動的模型，蛇體相鄰關節(jié)的相對轉角可用公式(1)表示機器人的運動狀態(tài):

圖5 蛇形機器人蜿蜒運動模型Fig.5 Snake-shaped robot turn motion model

式中α0表示蜿蜒運動的初始角度，n 代表機器人的單元模塊數(shù)量，l 表示單元模塊長度，L 表示機器人的整體長度，Kn表示蜿蜒運動時傳播的波的個數(shù)，K1表示蛇形機器人運動曲線的曲率偏差，s 表示蛇形機器人后部尾關節(jié)沿運動曲線軸線方向的虛位移，i 表示蛇形機器人中的任意關節(jié)，VL 表示擾動速度，Vd 表示目標速度.

蛇形機器人運動控制的基本方法是通過改變相鄰單元關節(jié)之間的相對轉角來控制蛇形機器人的運動姿態(tài)，上式表明了蛇形機器人虛位移與各相鄰單元關節(jié)間相對轉角φi的關系.當虛位移s 在蛇的運動過程中，隨時間變化而改變時，各個模塊的關節(jié)轉角也隨之發(fā)生改變，進而實現(xiàn)蛇形機器人的連續(xù)運動.在控制系統(tǒng)中，不斷根據(jù)s 的變化來實時計算關節(jié)轉角，后通過驅動信號使執(zhí)行舵機完成角度變換.

分布式控制將各個關節(jié)的轉角計算等運算問題分布在各自的從控制器上.主控制器將運動控制函數(shù)中的s 變量，通過CAN 總線網(wǎng)絡傳遞給各個單元關節(jié)的從控制器，由各自關節(jié)的從控制器自主計算自身關節(jié)的變換角度，并在完成全部運算后向主控制器發(fā)送一個確認信號，如圖6.根據(jù)蛇形機器人運動控制函數(shù)的特點，可以看出主要的計算變量是s 和i 在確定了單元關節(jié)中的硬件系統(tǒng)及各項參數(shù)之后，每個單元關節(jié)的i，對本關節(jié)從控制器來說是確定的，它只需要在CAN 總線上，獲取主控制器發(fā)送的s 變量，即可由自己根據(jù)算法來計算所需改變的角度.主控制器在接收到總線上所有的單元關節(jié)的完成計算確認后，發(fā)送一個同步信號，各單元關節(jié)根據(jù)同步信號，統(tǒng)一開始調整自身姿態(tài)，以實現(xiàn)蛇形機器人的運動調整.

圖6 分布式控制系統(tǒng)Fig.6 Distributed control system

采用分布式控制方式，將各關節(jié)的相對轉動角度計算交由各自從控制器來完成，這樣對主控制器的資源占用大大的降低，也可使節(jié)約出資源來處理如其他參數(shù)計算，運動規(guī)劃等任務［10，11］.分布式控制方式在機器人搭載的單元關節(jié)數(shù)越多時，表現(xiàn)的優(yōu)越性越明顯.依蛇形機器人搭載十六個單元關節(jié)為研究對象.當采用分布式控制，主控制器只傳遞變量s 至各個單元關節(jié)從控制器，由單元關節(jié)再根據(jù)自身的i 變量來計算所需變換的角度，所需時間約6ms，主控制器傳遞變量的周期可降低到37ms.

4 控制器的硬件設計

4.1 總體框架

管道探測蛇形機器人控制器主要由電源模塊、STM32 微控制模塊、攝像模塊、姿態(tài)傳感模塊、可燃氣體傳感器模塊、溫濕度傳感模塊、CAN 總線通信模塊組成［12，13］.

控制器以STM32 微處理器為核心，姿態(tài)傳感器將蛇形機器人實時的姿態(tài)解算并發(fā)送給STM32 主控芯片，主控芯片將信息上傳到上位機并實時顯示其俯仰角，橫滾角和航向角，使管理者或使用者實時掌握蛇形機器人的運動情況［14］.溫濕度傳感器將管道內部的溫度、濕度信息采集并傳送給主控芯片，經(jīng)主控芯片進行處理后上傳至上位機顯示，供管理者判斷是否符合溫濕度要求.攝像頭模塊也是最重要的模塊之一，將采集到的高清視頻信息經(jīng)主控芯片處理實時回傳并顯示到上位機界面，可以直觀地了解管道中是否存在缺陷、裂紋等異常情況.控制器還設置有CAN 總線通信模塊，方便主控制器與上位機的通信與控制［15］.

4.2 舵機驅動模塊

舵機是管道探測蛇形機器人構成中最重要的組成部分之一，其驅動信號是由主芯片輸出的PWM 信號來控制，高低電平是一種周期一定，占空比可調的方波，可以通過一個低通濾波器進行解調，通過控制脈沖寬度即可控制舵機旋轉角度，舵機通過外部供電，當接收到主控制器發(fā)送的PWM 信號時，蛇形機器人的各個關節(jié)開始運動.所選舵機型號為RDS3115MG，小巧且重量只有 60g，尺寸大小為 40×20×40.5mm，工作電壓4.8-7.2v，堵轉扭矩為15kg/cm.

蛇形機器人共有14 路舵機需要同時控制［16］，所以占用定時器的14 路輸出.而所選芯片包含12 個16 位的定時器和2 個32 位定時器，完全可以滿足任務要求.該蛇形機器人的14路舵機驅動信號由TIM1-TIM4 產(chǎn)生，具體對應如表1 所示.

表1 定時器PWM 信號通道與舵機對應表Table 1 Timer PWM signal channel and servo correspondence table

5 舵機控制程序設計

舵機與外界連接線有三端，分別是電源線、地線和控制線，其中只有控制線與控制器相連，由上位機下傳指令給控制器，控制器產(chǎn)生的PWM 信號傳輸給舵機，經(jīng)電路板上的IC處理后計算出轉動方向，再驅動馬達轉動，透過減速齒輪將動力傳至擺臂，同時由位置檢測器(電位器)返回位置信號，判斷是否已經(jīng)到設定位置.舵機控制需要一個高電平時基脈沖，該脈沖一般為0.5ms ～2.5ms 的角度控制脈沖.舵機轉過的角度為輸入脈寬控制，我們的舵機能在安裝固定件和連接件之后以225 度的角度旋轉，其對應的控制關系是這樣的:0.5ms—0 度;1.0ms—45 度;1.5ms—90 度;2.0ms—135 度;2.5ms—180 度;3ms—225 度;舵機控制模塊工作流程圖如圖7 所示.

圖7 舵機控制流程圖Fig.7 Steering gear control flow chart

舵機通電啟動之后，定時器開始初始化，控制器開始檢測舵機旋轉標志位是否等于1.若為1，表示有控制舵機的信號傳入，然后清零標志位，開始比較舵機實際位置與目標位置偏差，將偏差信號上傳至控制器來調整PWM 信號，進而更好地控制舵機.

6 系統(tǒng)測試

6.1 蛇形機器人控制器測試

通過排針的端子與TJA1050 收發(fā)芯片相連，之后由接線端子(CAN)與外部的CAN 總線相連.由于CAN 和USB 是共用PA11 和PA12 端口，所以用 CAN 傳輸時，要將開發(fā)板 P9 上的PA11 和PA12 用跳線帽分別與CAN_RX 和CAN_TX 相接.

將所選舵機供電之后，并與控制器相連，由于我們設計的控制器是同時輸出多路PWM，即控制多路舵機協(xié)調工作，設置控制舵機角度程序并下載到控制器，經(jīng)測試控制器可精確控制舵機角度，基本實現(xiàn)蜿蜒運動.控制器如圖8 所示.

圖8 控制器Fig.8 Controller

6.2 整體功能測試及上位機界面顯示

圖9 蛇形機器人及上位機界面Fig.9 Serpentine robot and host computer interface

文章為分布式控制，將設計好的小型控制器嵌入到蛇的每一節(jié)關節(jié)中，其中蛇頭為主控制器，其余關節(jié)為從控制器，蛇形機器人控制器通過CAN 總線連接到上位機，并根據(jù)需求完成上位機(組態(tài)王)顯示界面設計.經(jīng)調試后，在組態(tài)王按鍵和視頻插件中編寫腳本語言來實現(xiàn)所需功能，可在組態(tài)王視頻顯示區(qū)實時查看蛇形機器人拍攝的管內周圍環(huán)境，并可以實現(xiàn)縮放、拍照及保存功能.隨著蛇形機器人的運動，可以實時顯示蛇關節(jié)的俯仰角、橫滾角和航向角.同時，安裝在蛇身上的溫度傳感器和甲烷濃度傳感器等也將管內參數(shù)實時上傳至上位機顯示.由于在實驗室測試并沒有甲烷，所以甲烷濃度顯示0%，另外可以將蛇身上面的溫度傳感器值上傳到組態(tài)王并實時顯示，如圖9 所示為蛇形機器人實物及上位機界面.

7 結 論

利用蛇形機器人的靈活性和可拆卸性，通過控制調整舵機的轉角，使蛇形機器人不僅可適應大管徑也可適應小管徑進行探測，不僅可以在管道內部進行水平探測，并通過螺旋方式可以向上攀爬達到探測豎直管道的效果，即使在管徑較小的管道中，蛇形機器人也可以以直線的方式進行穿管并探測.文章對管道探測的蛇形機器人的控制系統(tǒng)進行了設計，對控制器進行了設計與開發(fā)，該控制器小巧、靈活，有效減小了蛇關節(jié)的大小，其上搭載的各類傳感器和通信接口均合理分配，可實時控制蛇的姿態(tài)以及實時檢測管道內的周圍環(huán)境、溫度等.

目前在燃氣管道中的探測蛇形機器人是沒有先例的，在燃氣管道鋪設越來越多的今天，利用蛇形機器人可以大大提高檢測效率如燃氣泄漏等問題.