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

（1.馬鞍山學(xué)院大阪醫(yī)工學(xué)院，安徽 馬鞍山 243100；2沈陽建筑大學(xué)，遼寧 沈陽 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ī)器人通過不同的移動(dòng)方式可以到不同的區(qū)域進(jìn)行移動(dòng)作業(yè)，因此研究機(jī)器人底盤結(jié)構(gòu)全向移動(dòng)控制意義重大。

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

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

因此，研究了爬壁機(jī)器人底盤結(jié)構(gòu)全向移動(dòng)自主控制方法，利用卡爾曼濾波器預(yù)測、觀測、更新解算機(jī)器人姿態(tài)，以提高移動(dòng)穩(wěn)定性，在爬壁機(jī)器人運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型的基礎(chǔ)上，通過PID控制算法離散處理輸入誤差，實(shí)現(xiàn)爬壁機(jī)器人底盤結(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)系到場地坐標(biāo)系之間存在的旋轉(zhuǎn)矩陣，其表達(dá)式為：

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

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

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

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

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

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

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

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

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

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

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

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

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

3.1 姿態(tài)解算

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

為了驗(yàn)證爬壁機(jī)器人底盤結(jié)構(gòu)全向移動(dòng)自主控制設(shè)計(jì)方法的整體有效性，需要對爬壁機(jī)器人底盤結(jié)構(gòu)全向移動(dòng)自主控制設(shè)計(jì)方法進(jìn)行測試，本次測試在MATLAB 7.0平臺(tái)中完成。設(shè)計(jì)的機(jī)器人，所有四個(gè)車輪都通過軸承機(jī)構(gòu)被動(dòng)旋轉(zhuǎn)，而電機(jī)則放在前面，以實(shí)現(xiàn)各個(gè)車輪的轉(zhuǎn)向。用于連接的碳纖維管直徑為10mm，在傾斜伺服電機(jī)中內(nèi)置過載傳感器，該模型的角分辨率為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ī)器人底盤結(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)行測試。設(shè)置平穩(wěn)性因子，在區(qū)間[1，10]內(nèi)取值，平穩(wěn)性因子越高表明控制方法的平穩(wěn)性越好。平穩(wěn)因子計(jì)算公式為：

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

平穩(wěn)不同方法的測試結(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)榉椒?利用卡爾曼濾波器對爬壁機(jī)器人的姿態(tài)進(jìn)行解算，解算過程通過預(yù)測、觀測及更新等步驟及時(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)一步對方法1、方法2和方法3的整體有效性進(jìn)行測試，與規(guī)劃好的路徑對比，偏差越小精準(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)行測試時(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é)束語

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