雙電機驅動的管道檢測機器人空間定位

王魯輝 李 巍 李生文 李太江 程曄鋒 徐向前

（1. 西安石油大學材料科學與工程學院，陜西 西安 710000；2. 西安熱工研究院有限公司，陜西 西安 710000）

0 引言

管道運輸是石油和天然氣等化工產品和原料的重要運輸方式，近幾十年發展迅速。截至2021年底，全球在役管道總里程約202萬公里，隨著人類對石油、天然氣需求量不斷增加，全球范圍內管道鋪設總里程仍將持續上升。同時，管道在夜以繼日的運行消耗后，對維護人員來說檢測工作是一項巨大的挑戰[1-3]。

在早期的管道檢測工作中，作業人員一般都采用整體挖掘法，將埋藏整條管道的土壤挖開檢查。傳統的檢測手段，不僅檢查效率低，還非常消耗人力、物力，檢測的效果也不理想，很容易對運輸管道造成傷害并延誤管道的使用。因此，管道檢測工程開始逐漸使用管道檢測機器人來代替作業人員開展檢測工作[4-6]。但是，對于管道檢測機器人而言，準確的管內定位技術是其正常穩定工作的前提條件之一，本文將針對雙電機驅動機器人在管道內的空間定位問題展開研究。

1 機器人電機的PID算法分析及仿真

1.1 PID算法分析

機器人電機驅動模塊的選擇至關重要，需要具有體積小、力矩大、噪音小等優點，同時還支持調速和正反轉功能。本研究中機器人采用型號為TBSM52-20B33的伺服電機，具體性能參數如表1所示。

表1 TBSM52-20B33伺服電機性能參數

為了提高電機的抗干擾能力，選擇串級PID控制器驅動控制電機工作，即速度環和電流環雙閉環設計。其控制框圖如圖1所示，該算法不僅可以抑制速度環路中的干擾，還可以抑制電流通道的干擾。輸入誤差根據系統閉環環路的組成可以分為實際速度測量值和速度標稱值之間的偏差，實際電流測量值和電流標稱值之間的偏差[7-9]。

圖1 電機串級PID控制結構

在時域上，PID控制算法可以描述為：

式中，Kp為比例常數，Ti為積分時間常數，Td為微分時間常數，均需要根據實際系統需要調節的參數，e（t）為誤差信號。在頻域上，對應的頻域方程為：

在PID控制算法的三個部分中，比例項在系統暫態時其主要作用，增大比例系數能夠提高響應速度，但同時也會導致系統超調，過大甚至會影響系統的穩定性，但單純的比例控制出現靜態誤差。因此，需要引入積分項消除靜態誤差，積分項主要在系統進入穩態后其主要作用，通過不斷累加誤差來達到消除誤差的目的，但是積分項的引入會使在控制初始階段超調量增加，系統出現滯后。為了避免過大的超調需要適當減弱積分作用。所以積分系數的選擇應綜合考慮超調量和靜態誤差的大小。微分項主要用來提前修正誤差，并能起到縮短調節時間和減小超調，同時也能夠抵消積分引入帶來的滯后問題，但是實際應用中一般不會加入微分項，因為通過差分的方式來等價微分的形式往往會放大系統中的干擾，影響系統的控制效果，所以速度環和電流環均采用的PI設計。

1.2 PID算法驗證

以速度環的PI控制器為例，說明PI參數的不停對于速度環控制的影響。直觀的表現就在于速度環的給定速度與經過PI控制器后的反饋速度的區別。

首先給定一組較好的參數作為示范Kp=2.3，Ki=38，分別對每個參數的影響進行仿真分析，同樣都在0.3秒時給予一個突變的速度500rpm。轉速的仿真結果如圖2所示。

圖2 Kp和Ki對控制系統的影響

比例控制是一種較為簡單的控制方式。其控制器的輸出與輸入誤差信號成比例關系。當僅有比例控制時系統輸出存在穩態誤差（b），明顯的可以看出了Ki對于系統最直觀的影響就是可以消減系統的穩態誤差。在積分控制中，控制器的輸出與輸入誤差信號的積分成正比關系。為了消除穩態誤差，在控制器中必須引入“積分項”。積分項對誤差取決于時間的積分，隨著時間的增加，積分項會增大。這樣，即便誤差很小，積分項也會隨著時間的增加而加大，它推動控制器的輸出增大使穩態誤差進一步減小，直到等于零。因此，比例+積分（PI）控制器，可以使系統在進入穩態后無穩態誤差（a）。

進一步，測試PI調節參數對控制性能的影響，最終效果如圖3所示。

圖3 PI參數對速度的影響

2 雙電機驅動的管道檢測機器人的定位研究

上述部分的測試，驗證了PID算法可減小控制時間，減小了穩態誤差，具備良好的速度控制性能。構建雙電機驅動的機器人，機器人共有四個車輪，前邊兩個車輪為驅動輪，后邊兩個車輪為從動輪用于支撐機器人，利用PID算法控制兩個驅動輪的速度，通過控制速度差實現機器人的左右轉向和直線行駛。在此基礎上，對雙電機驅動的機器人進行定位研究。

機器人內安裝有高精度編碼器和陀螺儀，編碼器用于測量速度和運行距離信息、陀螺儀用于獲得方位角測量信息。采用相對定位方式，基于位移、位置、方位角以及數學中的坐標變換原理對雙電機驅動的管道檢測機器人進行高精度空間定位[10,11]。

機器人姿態信息由安裝在機器人上的陀螺儀采集并送出，但由于機器人處于運動狀態時，陀螺儀也隨著機器人的運動而運動，從而使得由陀螺儀所確定的機器人自身坐標系不斷地變化。而變化的坐標系不便于對機器人坐標的表示，因此在機器人初始位置所在的坐標位置建立世界坐標系，該世界坐標系與機器人初始位置的坐標系重合，如圖4（a）所示，以垂直地面向上的方向為Z軸正方向，Y軸正方向為機器人此時的朝向，Y軸順時針方向旋轉九十度后與X軸重合。機器人運動后某一時刻世界坐標系與機器人自身坐標系的關系如圖4（b）所示。

圖4 坐標系關系

2.1 雙電機驅動的機器人空間位移計算

現規定將機器人前兩車輪的中點坐標位置視作機器人本身的坐標位置，位移大小L表示該中點的位移大小。以下將對機器人位移計算展開分析，本文中機器人的轉向是利用左右兩輪速度差完成的，機器人通過定義以下變量，輔助位移大小L的計算。

D：機器人兩車輪間距；

L1：機器人左輪通過路程；

L2：機器人右輪通過路程；

θ：機器人轉向時車輪走過的圓弧所對應的圓心角；

θ1、θ2：分別為機器人左右車輪轉動的總角度；

R：機器人轉向時左車輪路徑所在圓的半徑；

r：機器人車輪半徑。

機器人的運動大致可分為三種情況：左右兩車輪速度相同的直行、左右兩車輪差速同向轉動的轉彎、左右兩車輪轉動方向相反的轉彎。

（1）左右兩車輪速度相同的直行

此時左右兩車輪的移動距離即為機器人所移動的位移大小，前進或后退位移大小滿足：

（2）左右兩車輪差速同向轉動的轉彎

以機器人右輪轉速大于左輪進行差速向左轉向時為例，位移大小L如圖5所示。

圖5 機器人轉彎原理圖

由可知，機器人差速向左轉向時位移大小L近似等于圓心角θ所對應的圓弧長度：

機器人實際運動時，陀螺儀和編碼器采集數據的頻率可達到50Hz，機器人轉彎時，單位時間內機器人車輪走過的圓弧所對應的圓心角θ趨近于0，所以圓心角θ與其反正切值近似相等。可近似等價為機器人在單位時間內做了一次沿機器人前進方向的勻速直線運動，此時等價的位移大小L如圖6所示。

圖6 圓心角的計算原理

由圖3可以得到：

等價的位移大小L由計算：

考慮到機器人向不同方向轉彎時，L1與L2的大小關系會發生變化，當機器人向左轉向時L1L2。因此，當機器人進行左右兩車輪同向差速轉向時，位移大小L可由下式給出：

（3）左右兩車輪轉動方向相反的轉彎

當機器人進行左右兩車輪轉動方向相反的轉向時又包含兩類情況，即：L1等于L2和L1不等于L2。

當左車輪后退右車輪前進且行進距離L1等于L2，機器人轉動情況如圖7所示。

圖7 兩車輪反向轉動情況

可以看出在此種狀態下，機器人的中心位置沒有發生移動。此時機器人位移大小為零：

機器人轉過的圓心角：

當左車輪后退右車輪前進且行進距離L1不等于L2時機器人轉動情況如圖8所示。

圖8 兩車輪反向轉動且前進距離不同時的轉動情況

此時圓心角和位移滿足：

因此，當機器人進行左右兩車輪轉動方向相反的轉向時的位移大小如下：

2.2 雙電機驅動的機器人空間位置計算

由于機器人的速度、加速度、位移等的表示均是基于機器人坐標系，不能直接與世界坐標系內的變量進行運算。選取的陀螺儀恰好能夠給出機器人坐標系相對于世界坐標系三個坐標軸方向的旋轉量，根據該旋轉量由旋轉矩陣對機器人坐標系內的向量進行處理即可實現對機器人坐標系內的向量在世界坐標系內的投影顯示。

同樣規定將機器人前兩車輪的中點坐標位置視作機器人本身的坐標位置，以下說明通過旋轉矩陣怎樣實現位置的計算。由陀螺儀采集對應X、Y、Z坐標軸方向的旋轉角度量，分別為α，β，γ。

坐標系單獨繞X，Y，Z軸旋轉的旋轉矩陣如下：

由歐拉角表示姿態時坐標系旋轉順序定義為Z-Y-X，將總的旋轉矩陣記為R=Rz×Ry×Rx，旋轉矩陣可反映出機器人的姿態變化。

同時，通過計算可以由偏轉角度量α，β，γ得到機器人單位時間內的位移大小在世界坐標系三個坐標軸方向上的比重xi、yi、zi滿足：

對上述三個坐標軸方向上比重進行單位化處理得Xi、Yi、Zi，可得：

位移大小對于坐標的確定也有著影響，記一時間段內機器人通過的位移大小為L,機器人在該時間段內的位移大小在三個坐標軸方向上的分量△X、△Y、△Z由計算可得：

結合上式，通過坐標的累加，可得到機器人當前坐標（Xt+1，Yt+1，Zt+1），計算如下：

2.3 雙電機驅動的管道檢測機器人的定位算法驗證

為了驗證以上定位算法的可行性，控制機器人分別在平面上直線運動、圓周運動以及在管道外部沿管壁圓周運動，規定機器人左右兩輪連線的中點位置視為機器人所在位置。

首先，分別對機器人三種遠動過程，采集30組實際軌跡點，通過Origin軟件對實際運動軌跡進行了仿真處理，如圖9～圖11中的實際值所示。

圖9 直線運動仿真軌跡

圖10 圓周運動仿真軌跡

圖11 管壁外圓周運動仿真軌跡

其次，通過雙電機驅動管道檢測機器人定位算法，分別對運動過程的位置坐標進行計算，取30組位置坐標，通過Origin軟件對運動軌跡進行了模擬仿真，如圖9～圖11中的模擬值所示。

以下三圖給出了機器人實際運動軌跡（實際值）與基于雙電機驅動機器人定位算法所得運動軌跡（模擬值）的仿真結果對比。

觀察圖中軌跡可得，兩個軌跡基本重疊；后對每組實際軌跡與算法計算所得軌跡的30組位置坐標進行了誤差計算，求得平均誤差小于3.5%，屬于合理誤差范圍內。因此，驗證了該算法應用于雙電機驅動機器人空間定位具有較好的可靠性。

3 結語

（1）構建雙電機驅動的機器人模型，通過PID算法能夠有效控制機器人驅動輪的差速運轉，實現機器人左右轉向和直線運動；

（2）基于所推空間位置算法，開展雙電機驅動機器人管道定位試驗。控制檢測機器人在平面上直線運動、圓周運動以及在管道外部沿管壁圓周運動，對比定位算法所得仿真軌跡與實際運動仿真軌跡，并進行了誤差分析。最終驗證了該定位算法應用于雙驅機器人管道空間定位時的可靠性。