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

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

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

0 引言

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

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

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

1.1 PID算法分析

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

表1 TBSM52-20B33伺服電機性能參數(shù)

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

圖1 電機串級PID控制結(jié)構(gòu)

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

式中，Kp為比例常數(shù)，Ti為積分時間常數(shù)，Td為微分時間常數(shù)，均需要根據(jù)實際系統(tǒng)需要調(diào)節(jié)的參數(shù)，e（t）為誤差信號。在頻域上，對應(yīng)的頻域方程為：

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

1.2 PID算法驗證

以速度環(huán)的PI控制器為例，說明PI參數(shù)的不停對于速度環(huán)控制的影響。直觀的表現(xiàn)就在于速度環(huán)的給定速度與經(jīng)過PI控制器后的反饋速度的區(qū)別。

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

圖2 Kp和Ki對控制系統(tǒng)的影響

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

進(jìn)一步，測試PI調(diào)節(jié)參數(shù)對控制性能的影響，最終效果如圖3所示。

圖3 PI參數(shù)對速度的影響

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

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

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

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

圖4 坐標(biāo)系關(guān)系

2.1 雙電機驅(qū)動的機器人空間位移計算

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

D：機器人兩車輪間距；

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

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

θ：機器人轉(zhuǎn)向時車輪走過的圓弧所對應(yīng)的圓心角；

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

R：機器人轉(zhuǎn)向時左車輪路徑所在圓的半徑；

r：機器人車輪半徑。

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

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

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

（2）左右兩車輪差速同向轉(zhuǎn)動的轉(zhuǎn)彎

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

圖5 機器人轉(zhuǎn)彎原理圖

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

機器人實際運動時，陀螺儀和編碼器采集數(shù)據(jù)的頻率可達(dá)到50Hz，機器人轉(zhuǎn)彎時，單位時間內(nèi)機器人車輪走過的圓弧所對應(yīng)的圓心角θ趨近于0，所以圓心角θ與其反正切值近似相等?？山频葍r為機器人在單位時間內(nèi)做了一次沿機器人前進(jìn)方向的勻速直線運動，此時等價的位移大小L如圖6所示。

圖6 圓心角的計算原理

由圖3可以得到：

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

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

（3）左右兩車輪轉(zhuǎn)動方向相反的轉(zhuǎn)彎

當(dāng)機器人進(jìn)行左右兩車輪轉(zhuǎn)動方向相反的轉(zhuǎn)向時又包含兩類情況，即：L1等于L2和L1不等于L2。

當(dāng)左車輪后退右車輪前進(jìn)且行進(jìn)距離L1等于L2，機器人轉(zhuǎn)動情況如圖7所示。

圖7 兩車輪反向轉(zhuǎn)動情況

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

機器人轉(zhuǎn)過的圓心角：

當(dāng)左車輪后退右車輪前進(jìn)且行進(jìn)距離L1不等于L2時機器人轉(zhuǎn)動情況如圖8所示。

圖8 兩車輪反向轉(zhuǎn)動且前進(jìn)距離不同時的轉(zhuǎn)動情況

此時圓心角和位移滿足：

因此，當(dāng)機器人進(jìn)行左右兩車輪轉(zhuǎn)動方向相反的轉(zhuǎn)向時的位移大小如下：

2.2 雙電機驅(qū)動的機器人空間位置計算

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

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

坐標(biāo)系單獨繞X，Y，Z軸旋轉(zhuǎn)的旋轉(zhuǎn)矩陣如下：

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

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

對上述三個坐標(biāo)軸方向上比重進(jìn)行單位化處理得Xi、Yi、Zi，可得：

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

結(jié)合上式，通過坐標(biāo)的累加，可得到機器人當(dāng)前坐標(biāo)（Xt+1，Yt+1，Zt+1），計算如下：

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

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

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

圖9 直線運動仿真軌跡

圖10 圓周運動仿真軌跡

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

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

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

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

3 結(jié)語

（1）構(gòu)建雙電機驅(qū)動的機器人模型，通過PID算法能夠有效控制機器人驅(qū)動輪的差速運轉(zhuǎn)，實現(xiàn)機器人左右轉(zhuǎn)向和直線運動；

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