郭家偉，談 波，陳 華，吳 昊

（1.馬鞍山學(xué)院大阪醫(yī)工學(xué)院，安徽 馬鞍山 243100；2沈陽(yáng)建筑大學(xué)，遼寧 沈陽(yáng) 110168）

1 引言

機(jī)器人作為電子與機(jī)械的結(jié)構(gòu)化和智能化最好的體現(xiàn)，被廣泛地應(yīng)用在生活和生產(chǎn)中[1]。在不同的領(lǐng)域中機(jī)器人可以代替或協(xié)助人工完成各種復(fù)雜的任務(wù)，尤其在垂直作業(yè)環(huán)境中，爬壁機(jī)器人有效降低作業(yè)風(fēng)險(xiǎn)、提高工作效率[2-4]。爬壁機(jī)器人通過(guò)不同的移動(dòng)方式可以到不同的區(qū)域進(jìn)行移動(dòng)作業(yè)，因此研究機(jī)器人底盤(pán)結(jié)構(gòu)全向移動(dòng)控制意義重大。

目前研究通過(guò)底盤(pán)電機(jī)控制機(jī)器人全向移動(dòng)的方法較多，文獻(xiàn)[5]利用旋轉(zhuǎn)電位器計(jì)算機(jī)器人底盤(pán)單元對(duì)應(yīng)的偏轉(zhuǎn)角度，通過(guò)電機(jī)驅(qū)動(dòng)器控制底盤(pán)運(yùn)動(dòng)，實(shí)現(xiàn)了全向移動(dòng)機(jī)器人的控制。文獻(xiàn)[6]將可編程邏輯控制器作為機(jī)器底盤(pán)的控制器，以研究輪轂電動(dòng)機(jī)和伺服電動(dòng)機(jī)的運(yùn)動(dòng)控制為主，并對(duì)其進(jìn)行運(yùn)動(dòng)學(xué)分析，根據(jù)分析結(jié)果同步控制8個(gè)電動(dòng)機(jī)，實(shí)現(xiàn)移動(dòng)機(jī)器人的移動(dòng)控制。文獻(xiàn)[7]以機(jī)器人瞬時(shí)旋轉(zhuǎn)中心作為切入點(diǎn)進(jìn)行研究。將底盤(pán)視為一個(gè)剛體，提出了一種估計(jì)關(guān)節(jié)空間內(nèi)底盤(pán)瞬時(shí)旋轉(zhuǎn)中心的算法，精確計(jì)算底盤(pán)瞬時(shí)旋轉(zhuǎn)中心的位置估計(jì)值，從而實(shí)現(xiàn)底盤(pán)全向移動(dòng)的控制。

在機(jī)器人運(yùn)動(dòng)學(xué)約束方面，文獻(xiàn)[8]提出一種爬壁機(jī)器人準(zhǔn)全向控制方法，考慮粘附系統(tǒng)運(yùn)動(dòng)學(xué)約束，通過(guò)預(yù)定的模糊控制方法最大化機(jī)器人的可操作性，減少結(jié)構(gòu)的動(dòng)態(tài)干擾，做到機(jī)器人平穩(wěn)導(dǎo)航；同時(shí)也有文獻(xiàn)[9]考慮工業(yè)檢測(cè)環(huán)境，以爬壁機(jī)器人無(wú)損檢測(cè)為前提，裝配便攜式傳感器以提高運(yùn)動(dòng)與黏著機(jī)制的適應(yīng)性。以上研究成果在其設(shè)定的環(huán)境中取得了較好的機(jī)器人移動(dòng)控制結(jié)果，總體來(lái)說(shuō)，在爬壁機(jī)器人運(yùn)動(dòng)姿態(tài)的穩(wěn)定控制與路徑控制精度方面可以進(jìn)一步完善。

因此，研究了爬壁機(jī)器人底盤(pán)結(jié)構(gòu)全向移動(dòng)自主控制方法，利用卡爾曼濾波器預(yù)測(cè)、觀測(cè)、更新解算機(jī)器人姿態(tài)，以提高移動(dòng)穩(wěn)定性，在爬壁機(jī)器人運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型的基礎(chǔ)上，通過(guò)PID控制算法離散處理輸入誤差，實(shí)現(xiàn)爬壁機(jī)器人底盤(pán)結(jié)構(gòu)全向移動(dòng)的自主控制。

2 機(jī)器人運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)建模

2.1 坐標(biāo)系變換

因爬壁機(jī)器人的移動(dòng)方式是平行移動(dòng)及旋轉(zhuǎn)移動(dòng)，所以根據(jù)運(yùn)動(dòng)需求精準(zhǔn)轉(zhuǎn)換坐標(biāo)系。設(shè)R(θ)代表的是機(jī)器人體坐標(biāo)系到場(chǎng)地坐標(biāo)系之間存在的旋轉(zhuǎn)矩陣，其表達(dá)式為：

式中：θ—機(jī)器人兩個(gè)坐標(biāo)x的姿態(tài)角。設(shè)R-1(θ)代表的是旋轉(zhuǎn)矩陣R(θ)對(duì)應(yīng)的逆矩陣，實(shí)現(xiàn)逆向行駛，其表達(dá)式為：

設(shè)Vx、Vy、W代表的是場(chǎng)地坐標(biāo)系下存在的速度指令；vx、vy、w代表的是機(jī)器人體坐標(biāo)系下存在的速度質(zhì)量，兩者之間的轉(zhuǎn)換關(guān)系如下：

通過(guò)式（4）將機(jī)器人體坐標(biāo)系中存在的速度指令vx、vy、w轉(zhuǎn)變到場(chǎng)地坐標(biāo)系下存在的速度指令Vx、Vy、W：

至此，實(shí)現(xiàn)爬壁機(jī)器人體坐標(biāo)系與場(chǎng)地坐標(biāo)系的變換。

2.2 機(jī)器人動(dòng)力學(xué)模型

機(jī)器人全向輪在無(wú)滑動(dòng)條件下的速度分配受幾何分布的影響，在無(wú)滑動(dòng)條件下機(jī)器人底盤(pán)四個(gè)輪子的速度分配方案是唯一的：

式中：Ri(i=1,2,3,4)—機(jī)器人中心與第i個(gè)全向輪質(zhì)心之間存在的距離。在剛體轉(zhuǎn)動(dòng)定律的基礎(chǔ)上獲得爬壁機(jī)器人底盤(pán)全向輪的轉(zhuǎn)動(dòng)方程：

式中：Ti—驅(qū)動(dòng)力矩；fi—摩擦力；j—全向輪轉(zhuǎn)動(dòng)慣量；αi—全向輪對(duì)應(yīng)的角加速度。

在全向輪坐標(biāo)系下，機(jī)器人在運(yùn)動(dòng)過(guò)程中受到四個(gè)全向輪的牽引力，以及在機(jī)器人體坐標(biāo)系下全向輪的轉(zhuǎn)動(dòng)力矩和牽引力分量之間存在下述關(guān)系：

式中：Fx、Fy、Tw—機(jī)器人受到的牽引力或力矩。

聯(lián)立公式（7），獲得爬壁機(jī)器人的動(dòng)力學(xué)模型：

式中：M—車(chē)體質(zhì)量；J—車(chē)體轉(zhuǎn)動(dòng)慣量。

3 全向移動(dòng)自主控制設(shè)計(jì)

3.1 姿態(tài)解算

爬壁機(jī)器人底盤(pán)結(jié)構(gòu)全向移動(dòng)自主控制設(shè)計(jì)方法利用卡爾曼濾波器對(duì)爬壁機(jī)器人的姿態(tài)進(jìn)行解算。設(shè)z∈Rn代表的是離散時(shí)間狀態(tài)變量，通過(guò)卡爾曼濾波器對(duì)變量z進(jìn)行估計(jì)[10]，通過(guò)下述公式描述變量z的離散隨機(jī)方差方程：

式中：—移動(dòng)過(guò)程激勵(lì)噪聲。

爬壁機(jī)器人底盤(pán)結(jié)構(gòu)全向移動(dòng)自主控制設(shè)計(jì)方法為了方便計(jì)算通過(guò)四元數(shù)進(jìn)行表示：

式中：q1—四元數(shù)的實(shí)部；q2、q3、q4—四元數(shù)的三維虛部。

在上述計(jì)算的基礎(chǔ)上，爬壁機(jī)器人底盤(pán)結(jié)構(gòu)全向移動(dòng)自主控制設(shè)計(jì)方法利用卡爾曼濾波器對(duì)爬壁機(jī)器人的姿態(tài)進(jìn)行解算的過(guò)程如下：

（1）預(yù)測(cè)：用四元數(shù)代替角向量的過(guò)程如下：

對(duì)式（11）中存在的變量進(jìn)行積分處理，對(duì)狀態(tài)值進(jìn)行預(yù)測(cè)：

式中：δt—時(shí)間采樣周期；F—狀態(tài)變換矩陣。

為了模擬移動(dòng)過(guò)程噪聲，需要對(duì)機(jī)器人陀螺儀各軸對(duì)應(yīng)的方差進(jìn)行估計(jì)，根據(jù)估計(jì)結(jié)果通過(guò)式（13）計(jì)算矩陣Q：

（2）觀測(cè)：機(jī)器人系統(tǒng)當(dāng)前的狀態(tài)可以通過(guò)觀測(cè)值進(jìn)行反映，通過(guò)測(cè)量磁力計(jì)信號(hào)和測(cè)量加速度計(jì)獲得機(jī)器人系統(tǒng)的觀測(cè)值。狀態(tài)空間可以通過(guò)矩陣Q映射到觀測(cè)空間中。

（3）更新：預(yù)測(cè)狀態(tài)可以通過(guò)加權(quán)求和進(jìn)行更新：

式中：K—卡爾曼濾波器增益，其計(jì)算公式為：

通過(guò)上述計(jì)算過(guò)程，可以實(shí)時(shí)預(yù)測(cè)、觀測(cè)并更新爬壁機(jī)器人的姿態(tài)，為移動(dòng)自主控制提供基礎(chǔ)姿態(tài)數(shù)據(jù)。

3.2 移動(dòng)自主控制

爬壁機(jī)器人底盤(pán)結(jié)構(gòu)全向移動(dòng)自主控制設(shè)計(jì)方法通過(guò)PID（Proportion Integration Differentiation）控制算法對(duì)爬壁機(jī)器人進(jìn)行控制。PID控制器是通過(guò)線性組合微分環(huán)節(jié)、積分環(huán)節(jié)和比例環(huán)節(jié)構(gòu)成的系統(tǒng)輸入控制量，通過(guò)控制量對(duì)爬壁機(jī)器人進(jìn)行控制[11]。通過(guò)公式（26）描述PID控制器的控制規(guī)律：

式中：u(t)—PID 控制器的輸出；u0—控制常量；TD—微分常數(shù)；TI—積分常數(shù)；e(t)—PID控制器的輸入，可通過(guò)輸出值y(t)與r(t)計(jì)算得到。

爬壁機(jī)器人底盤(pán)結(jié)構(gòu)全向移動(dòng)自主控制設(shè)計(jì)方法利用增量的形式代替微分，用累加的方式代替積分，在PID連續(xù)公式中引入離散處理的形式為：

式中：ek—在第k次采樣中系統(tǒng)對(duì)應(yīng)的輸入誤差；ek-1—在第k-1次采樣中系統(tǒng)對(duì)應(yīng)的輸入誤差；T—采樣周期；TI—離散采樣時(shí)間；uk—在第k次采樣中系統(tǒng)對(duì)應(yīng)的輸出值；u0—系統(tǒng)初始的控制值；KD、KI—微分環(huán)節(jié)系數(shù)、積分環(huán)節(jié)系數(shù)。

通過(guò)上述公式，獲得第k-1個(gè)采樣時(shí)刻控制器對(duì)應(yīng)的輸出值：

整理上述公式，獲得增量式PID控制算法公式，實(shí)現(xiàn)爬壁機(jī)器人底盤(pán)結(jié)構(gòu)全向移動(dòng)的自主控制：

4 實(shí)驗(yàn)結(jié)果與分析

4.1 實(shí)驗(yàn)環(huán)境

為了驗(yàn)證爬壁機(jī)器人底盤(pán)結(jié)構(gòu)全向移動(dòng)自主控制設(shè)計(jì)方法的整體有效性，需要對(duì)爬壁機(jī)器人底盤(pán)結(jié)構(gòu)全向移動(dòng)自主控制設(shè)計(jì)方法進(jìn)行測(cè)試，本次測(cè)試在MATLAB 7.0平臺(tái)中完成。設(shè)計(jì)的機(jī)器人，所有四個(gè)車(chē)輪都通過(guò)軸承機(jī)構(gòu)被動(dòng)旋轉(zhuǎn)，而電機(jī)則放在前面，以實(shí)現(xiàn)各個(gè)車(chē)輪的轉(zhuǎn)向。用于連接的碳纖維管直徑為10mm，在傾斜伺服電機(jī)中內(nèi)置過(guò)載傳感器，該模型的角分辨率為0.323°，0.166s/60°的速度以及12 kg f.cm的失速扭矩，工作角度為320°并支持連續(xù)旋轉(zhuǎn)。并集成了用于角運(yùn)動(dòng)和轉(zhuǎn)矩控制的PID控制器，能夠定義其速度、最小/最大操作角度以及影響其通信協(xié)議的其他參數(shù)。

4.2 實(shí)驗(yàn)與分析

分別采用爬壁機(jī)器人底盤(pán)結(jié)構(gòu)全向移動(dòng)自主控制設(shè)計(jì)方法（方法1）、基于ST Nucleo-F446RE 的全向移動(dòng)機(jī)器人控制方法（方法2）和基于運(yùn)動(dòng)學(xué)分析的全向移動(dòng)機(jī)器人控制方法（方法3）進(jìn)行測(cè)試。設(shè)置平穩(wěn)性因子，在區(qū)間[1，10]內(nèi)取值，平穩(wěn)性因子越高表明控制方法的平穩(wěn)性越好。平穩(wěn)因子計(jì)算公式為：

式中：Ru—機(jī)器人重心到旋轉(zhuǎn)軸的距離；θ—Ru與垂直平面的夾角；φ—水平面旋轉(zhuǎn)傾斜角。

平穩(wěn)不同方法的測(cè)試結(jié)果，如圖1所示。

圖1 不同方法的平穩(wěn)性因子Fig.1 Stationarity Factors of Different Methods

分析圖1可知，方法1在多次迭代中獲得的平穩(wěn)性因子平均在7以上，遠(yuǎn)遠(yuǎn)高于方法2和方法3在多次迭代中獲得的平穩(wěn)性因子。平穩(wěn)性因子越高，表明方法控制的爬壁機(jī)器人平穩(wěn)性越好。因?yàn)榉椒?利用卡爾曼濾波器對(duì)爬壁機(jī)器人的姿態(tài)進(jìn)行解算，解算過(guò)程通過(guò)預(yù)測(cè)、觀測(cè)及更新等步驟及時(shí)調(diào)整校正機(jī)器人姿態(tài)，在爬壁機(jī)器人姿態(tài)信息的基礎(chǔ)上控制爬壁機(jī)器人的移動(dòng)，得到了機(jī)器人平穩(wěn)行進(jìn)的控制效果。

將控制精準(zhǔn)度作為指標(biāo)，將實(shí)驗(yàn)平面傾斜角度12°的平面進(jìn)行精度控制實(shí)驗(yàn)，進(jìn)一步對(duì)方法1、方法2和方法3的整體有效性進(jìn)行測(cè)試，與規(guī)劃好的路徑對(duì)比，偏差越小精準(zhǔn)度越高?？刂坡窂角€實(shí)驗(yàn)環(huán)境，如圖2所示。

圖2 控制路徑曲線實(shí)驗(yàn)環(huán)境Fig.2 Experimental Environment of Control Path Curve

不同方法的控制路徑實(shí)驗(yàn)結(jié)果，如圖3所示。由圖3中的數(shù)據(jù)可知，采用方法1、方法2進(jìn)行測(cè)試時(shí)，與規(guī)劃路徑的偏差較大。而方法1控制的路徑與規(guī)劃路徑幾乎相同。因?yàn)榇舜卧O(shè)計(jì)的方法在姿態(tài)更新解算的基礎(chǔ)上，利用增量的形式代替微分，能及時(shí)減少路徑輸入誤差，將PID控制算法進(jìn)行離散處理，獲得增量化的控制算法，提高了機(jī)器人移動(dòng)路徑的控制精度。

圖3 不同方法的控制路徑曲線Fig.3 Control Path Curves of Different Methods

5 結(jié)束語(yǔ)

研究了爬壁機(jī)器人移動(dòng)控制的問(wèn)題，利用卡爾曼濾波器預(yù)測(cè)更新機(jī)器人姿態(tài)，引入PID 控制算法離散化處理移動(dòng)輸入誤差。經(jīng)實(shí)驗(yàn)驗(yàn)證可以得出以下結(jié)論：（1）在迭代250次的平穩(wěn)性實(shí)驗(yàn)中，方法通過(guò)卡爾曼濾波器解算姿態(tài)下獲得了較高的平穩(wěn)性因子，使得機(jī)器人的全向移動(dòng)穩(wěn)定性優(yōu)于對(duì)比方法；（2）在傾斜角度12°的平面沿既定路徑行進(jìn)時(shí)，通過(guò)PID控制提高了移動(dòng)路徑控制的精準(zhǔn)度，使得機(jī)器人行進(jìn)路徑更貼合實(shí)際規(guī)劃路徑，減少了路徑誤差，使路徑移動(dòng)控制性能優(yōu)于對(duì)比的兩種方法。（3）爬壁機(jī)器人在移動(dòng)過(guò)程中只通過(guò)底盤(pán)結(jié)構(gòu)的控制難以消除移動(dòng)平面不平造成的摩擦力負(fù)載環(huán)境的變化，因此需要在以后的工作和研究中考慮爬壁機(jī)器人位姿閉環(huán)控制器的設(shè)計(jì)。