管道探測蛇形機器人控制系統設計及仿真

邢利輝，王亞慧，鄧 蕊

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

1 引 言

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

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

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

2.1 蛇形機器人關節設計

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

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

圖1 蛇關節剖面圖Fig.1 Snake joint planing

2.2 ADAMS 建模及仿真

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

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

圖2 250mm 管道蛇姿態圖Fig.2 250mm pipe snake attitude diagram

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

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

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

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

3 控制系統整體結構設計

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

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

3.1 基于CAN 總線的控制方法

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

圖4 控制系統整體框架Fig.4 Control system overall framework

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

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

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

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

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

圖6 分布式控制系統Fig.6 Distributed control system

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

4 控制器的硬件設計

4.1 總體框架

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

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

4.2 舵機驅動模塊

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

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

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

5 舵機控制程序設計

舵機與外界連接線有三端，分別是電源線、地線和控制線，其中只有控制線與控制器相連，由上位機下傳指令給控制器，控制器產生的PWM 信號傳輸給舵機，經電路板上的IC處理后計算出轉動方向，再驅動馬達轉動，透過減速齒輪將動力傳至擺臂，同時由位置檢測器(電位器)返回位置信號，判斷是否已經到設定位置.舵機控制需要一個高電平時基脈沖，該脈沖一般為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 系統測試

6.1 蛇形機器人控制器測試

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

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

圖8 控制器Fig.8 Controller

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

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

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

7 結 論

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

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