全方位機器人的重心位置預測與軌跡跟蹤控制

王義娜 ,劉賽男 ,王碩玉 ,楊俊友

(1.沈陽工業大學電氣工程學院,遼寧 沈陽 110870;2.高知工科大學智能機械系,日本 高知 7828502)

1 引言

與傳統的移動機器人相比,全方位移動機器人具有以同一姿勢向各方向移動的優點,在工業、醫學和航空航天領域應用廣泛,如搬運機器人[1]、步行訓練機器人[2]等.但是由于貨物放置位置不同、被輔助者姿勢發生變化會導致機器人重心產生偏移,進而影響機器人的軌跡跟蹤精度.為了進一步提高機器人的軌跡跟蹤精度,實時估計并預測重心位置具有重要作用.

全方向移動機器人屬于動態非線性強耦合系統,因此其跟蹤控制尤其是針對重心偏移的路徑跟蹤控制問題比較難.目前國內外學者針對此問題的研究主要集中于解決機器人的參數不確定性和外部擾動問題以提高其跟蹤精度[3–14],其中包括基于分層改進算法的模糊動態滑?？刂芠3]、基于擴展狀態觀測器的積分滑?？刂芠4]、模糊控制[5]、基于擾動觀測器的滑?？刂芠6]、基于極坐標的自適應控制[7]、基于雙魯棒控制器切換的控制算法[8]、自抗擾控制[9]、自抗擾反步控制[10]以及基于擾動觀測器的滑?？刂芠11]等算法.其中,文獻[3–6]是面向雙輪移動機器人而并非全方向移動機器人,文獻[7–11]研究未考慮重心偏移問題.另外,有些研究者基于模型預測控制展開研究[12–16],其中包括基于線性擴展狀態觀測器的模型預測控制[12]、具有控制約束的模型預測控制[13–14]以及應用摩擦補償的模型預測控制[15–16]、應用重心偏移補償的模型預測控制[17].這些控制算法可在滿足約束條件下減小重心偏移對跟蹤精度的影響,然而都未直面重心偏移引起的動態耦合矩陣不確定問題進行處理,不能完全解決重心偏移對跟蹤精度的影響.文獻[18]提出一種基于多轉子混合矩陣的估計法,對重心偏移產生的位置漂移進行補償,文獻[19]基于神經網絡對機器人重心位置進行估計,并結合自適應加權數據融合算法獲得其最終重心位置,然后通過反向動力學模塊對其進行運動控制.然而,文獻[18–19]的研究對象均為飛行機器人.專門針對全方位移動機器人重心偏移問題的路徑跟蹤控制方法包括具備參數辨識功能的自適應控制法[20]及基于統計對比模型的自適應跟蹤控制[21],然而文獻[20]中控制器的穩定性建立在重心位置變化較慢的基礎上,另外,文獻[21]中統計對比模型限制項較多,很難滿足要求.文獻[22]通過將重心偏移對繞重心轉矩的影響轉換到機器人的慣性矩陣中,通過最小二乘法在線辨識慣性參數,來補償重心偏移影響.然而,該方法辨識的慣性補償項是重心偏移引起的偏差項,存在滯后性.現有的基于數據驅動建模的重心偏移參數估計法在實時控制問題中應用研究較少,且存在滯后性,對估計結果做進一步預測,可提高控制時效[23–24].

針對以上問題,本文提出了一種基于數據驅動的系統建模法及基于重心參數預測的軌跡跟蹤控制法,主要貢獻歸納如下:

1)建立機器人考慮重心偏移的動力學模型,針對實時重心偏移問題,應用長短期記憶(long short-term memory,LSTM)神經網絡構建其對比模型,并基于數據驅動法對重心偏移參數進行估計,再基于張神經網絡(zhang neural network,ZNN)對估計參數進行預測,可解決估計滯后性的影響;

2)針對非線性動態強耦合問題,提出基于動態非線性反饋解耦法及數值加速度控制法的機器人軌跡跟蹤控制算法,保證控制系統穩定的同時實現機器人的高精度跟蹤.

2 OWTR及其動力學建模

本論文將以本實驗室所開發全方位步行訓練機器人(omnidirectional walking training robot,OWTR)為研究對象.如圖1所示,OWTR的四角底部安裝了4個全向輪,以實現全方向移動.OWTR的扶板內安裝了6個壓力傳感,以檢測使用者附加在扶板上的壓力.扶板前方安裝了觸摸屏,以作為人機交互接口,可供使用者選擇OWTR的多種訓練模式.另外,機器人還設計了多重安全保護措施,如:防后傾支撐結構、防墜坐墊與急停按鈕,以保證使用者的安全.

OWTR寬0.93 m,長1.03 m,自重為80 kg,最大負荷為80 kg,其扶板高度從0.9 m到1.2 m可調,以適應不同高度的訓練者.

為簡化分析,本論文將OWTR當成剛體并假設其受力及運動在同一二維水平面內.當使用者借助機器人行走時,由于姿勢及步態的改變,會導致機器人的重心位置實時變化.因此,建立其二維坐標系內分析圖如圖2 所示.其中:Σ(X,O,Y)為地球坐標系;Σ(xR,C,yR)為機器人坐標系;C為機器人中心位置;G為機器人重心位置,且重心偏移量可由r與α來描述,r為中心到重心間距離,α是CG相對于姿勢角的角度;θ為機器人姿勢角,定義為第1個全向輪同中心連線與x軸之間夾角;V指機器人的速度;β為機器人的移動方向,指V方向相對x軸的夾角,根據圖2所示受力情況,vi為第i個輪子的速度;l是機器人中心到4個全向輪的距離;fi指第i個全向輪所受驅動力;li指機器人重心到第i個驅動力的力臂,i=1,2,3,4.如圖2所示,當存在重心偏移時,各力臂長度會隨重心位置的變化而變化,從而影響繞重心位置的轉矩,另外還會引起機器人轉動慣量的變化,進而影響運動性能.因此設計控制輸入時,需要得到重心位置到各驅動力的距離,以消除重心偏移對運動精度的影響.

圖2 機器人受力及運動分析圖Fig.2 Force and motion analysis of robot

接下來本文將基于圖2建立機器人的非線性運動學及動力學模型.首先,建立機器人中心位置的運動學模型如下所示:

其中:M為機器人質量,m為人附加給機器人的等價質量,I為機器人的轉動慣量,mr2為重心偏移引起的附加轉動慣量,,為機器人重心位置在x方向與y方向的加速度,為機器人旋轉的角加速度.

在實際運行中,筆者更希望高精度控制機器人的中心位置,因此,需建立其中心位置的動力學模型.將等式(2)代入式(3)可得機器人重心處加速度和中心處加速度之間的關系如下:

最后,根據重心位置動力學模型(3)及重心與中心位置加速度關系(4),建立機器人中心位置的動力學模型如下:

由此動力學模型(5)可以看出,于重心與中心位置相對運動關系會產生一個切向力和法向力而影響相互運動,即出現擾動項MG影響中為位置的運動,該擾動項同樣會隨重心位置(r和α)的變化而變化.

3 控制器設計

本節針對具有重心偏移干擾和摩擦干擾的機器人系統,設計了一種考慮重心偏移的數值加速度控制方案.通過一種基于LSTM神經網絡訓練對比模型,根據對比模型到達的位置和機器人實際位置之間的差值估計重心參數,然后采用張神經預測求逆網絡進行參數預測并將結果引入控制律,控制系統結構框圖如圖3所示.

圖3 帶有參數估計及預測的數值加速度控制系統Fig.3 Digital acceleration control system with parameter estimation and prediction

3.1 考慮重心偏移的數值加速度控制方案

首先,基于文獻[22]所提數值加速度控制算法,通過將式(5)所示機器人動力學模型以采樣周期τ進行離散化,設計包含非線性狀態反饋的OWTR數值化加速度控制器,其表達式如下:

3.2 重心偏移量在線估計

由于式(6)中非線性狀態反饋矩陣中不確定項和機器人實時重心偏移量(rk和αk)相關,故本節引入一個新的參數估計法實時估計當前時刻重心偏移量(和),從而得到式(6)中非線性狀態反饋矩陣,的值.該參數估計法可由機器人4個輪子受到的驅動力和實時位置輸入得到機器人實時重心偏移量的估計值.

給定對比模型和實際模型相同驅動力F(k),U(k)=KC(k)F(k)為對比模型3個方向上的扭矩值,由對比模型和實際機器人之間的位置差?X(k)=(k)-XC(k)與當前時刻的重心偏移量關系建立如下等式:

式中除矩陣MG和T中含有未知的重心偏移參數rk,αk,其余量均為已知,故采用牛頓迭代法解方程(7)可對重心偏移量進行估計.

方程(7)可由以下步驟得出：

步驟1建立對比機器人模型k時刻位置(k)和4個全向輪上的驅動力F(k)關系式為

步驟2建立有重心偏移機器人模型k時刻位置和4個全向輪上的驅動力F(k)關系式為

式中:XC(k-1)為t=(k-1)τ時刻XC的值,(k-1)為t=(k-1)τ時刻˙XG的值,KG(k)為t=kτ時刻KG的值.

步驟3將式(8)–(9)做差,即可得到式(7).

3.3 基于LSTM建立對比模型

式(7)成立的前提為對比模型除沒有重心偏移影響外,其余摩擦等干擾均和實際機器人保持一致.故在實際應用中建立對比模型時需要考慮機器人行走過程中受到的干擾情況.對比模型通過LSTM神經網絡進行預訓練使其接近理想的對比模型.

LSTM神經網絡是一種以LSTM細胞為隱層的遞歸神經網絡(recursive neural network,RNN),原理圖如圖4所示,用公式表示如下:

圖4 LSTM神經網絡原理圖Fig.4 Schematic diagram of LSTM neural network

LSTM單元的輸入是t=kτ時刻對比模型4個輪子驅動力F(k),通過式(13)–(17)得到LSTM輸出:t=kτ時刻對比模型的位置(k).LSTM神經網絡可以提取隱藏的自然結構和抽象特征,且具有非線性擬合能力,確?；谠撋窠浘W絡建立的對比模型能有效估計出重心偏移參數.

3.4 重心偏移量預測

為了克服重心偏移量的影響,需要實時估計當前時刻的重心偏移量并提前進行補償,參數估計部分只能估計出上一時刻的重心偏移量,具有滯后性.為了解決這個問題,本文采用文獻[24]中的ZNN預測求逆方法對矩陣KG進行預測求逆,進而預測下一時刻的重心偏移量.ZNN預測求逆原理圖如圖5所示.

圖5 ZNN預測求逆原理圖Fig.5 Schematic diagram of ZNN prediction inversion

由圖5可得連續時間ZNN預測求逆公式如下:

采用四階有限差分公式近似離散KG的一階導數,即

由式(16)–(17)可得k+1時刻值如下:

其中:h=λτ為預測參數,其決定著重心參數估計的速度及精度;由k+1時刻的值可得k+1時刻重心偏移量.

3.5 穩定性分析

定理1假設系統重心偏移參數有界,則系統矩陣KG的弗洛貝尼烏斯(Frobenius)范數∥KG∥F有界,采用式(18)預測KG的廣義逆矩陣并得到重心偏移參數,假設式(16)所示的五階差分截斷誤差為o(τ3)及式(17)所示的四階瞬時后向有限差分截斷誤差為o(τ3)時,利用預測參數h,使預測參數趨近真實值,估計值且截斷誤差階為o(τ4),最大參數估計誤差為τ4,因此保證了閉環系統的穩定性.

證 為抑制因KG中含有重心偏移參數的項時變導致其廣義逆矩陣不確定而產生的影響,使用式(18)對進行估計,將其代入KG求逆誤差公式的Frobenius 范數中,由式(8)(17)得其截斷誤差各為o(τ3)可得

定理2對于考慮重心位置實時變化的OWTR動態模型(式(5)),在控制輸入(6)作用下,當控制參數滿足以下約束條件時:

證首先,建立OWTR動態模型(式(5))在時間區間[(k-1)τ+,kτ]邊緣時刻的離散化動力學模型為

將控制輸入(6)代入離散化動力學模型(20)–(21)中,并定義控制系統kτ+時刻的跟蹤誤差為

其中:XC(kτ+)為t=kτ+時刻機器人的實際位置.Xd(kτ+)為t=kτ+時刻機器人的目標位置,將跟蹤誤差(17)代入式(20)–(21)與式(6)構成的OWTR離散化閉環控制系統中,可得系統跟蹤誤差離散化動力學模型為

4 仿真驗證

通過仿真驗證本文提出的帶有參數估計及預測的數值加速度控制器的有效性.仿真中對比模型采用長短期記憶網絡進行訓練,先使機器人空載按預定的軌跡移動,記錄4個輪子的控制輸入與當前位置數據,采用長短期記憶神經網絡進行訓練,訓練結果作為對比模型,訓練結果如圖6所示.由圖6可知,在無外部干擾情況下LSTM神經網絡可準確近似與理論對比模型,由此可知實際應用中,LSTM神經網絡也能很好的逼近實際機器人的對比模型.

圖6 神經網絡訓練結果圖Fig.6 Neural network training results

為了驗證本文所提參數估計及預測方法的有效性,分別與文獻[20]所用自適應控制器及文獻[22]所提數值加速度控制器進行了仿真對比分析.自適應控制器參數為:Γ=diag{110,110,110},λ=diag{30,25,16},K=diag{140,140,6}.采樣時間為τ=0.04 s.兩個數值加速度控制器參數為:KP=diag{20,20,20},KD=diag{10,10,10}.重心參數估計與預測參數λ=0.3.

OWTR 初始位置設為XC0=[0 0.2 0]T,仿真時間為20 s,未知重心偏移參數設計如下:r=0.2+0.15 sint,α=0.02t,期望軌跡為

機器人的仿真結果如圖7 所示,運動路徑如圖7(a)所示.由圖7(b)可知,數值加速度控制的跟蹤誤差:x軸16 s 后才收斂至0.010 m,y軸17 s 后才收斂至0.011 m,θ也需17 s以上才收斂至0.827 rad.自適應控制的跟蹤誤差:x軸在4 s 內收斂至0.009 m,y軸用不到3 s 收斂至0.012 m,θ需13 s 以上收斂至0.280 rad.本文所提控制算法的跟蹤誤差:x軸由1 s 內收斂至0.004 m以下,y軸用不到1.5 s收斂至0.00 5m以內,θ用不到1.5 s收斂至0.038 rad 以內.相對于數值加速度與自適應控制器,本文所提控制算法跟蹤誤差更小,響應時間更快.表明所提控制算法由圖7(c)所示,估計值t12,t13在4 s內即可逼近于真值,穩定后估計誤差絕對值保持在0.001范圍內,驗證所提算法能正確估計重心偏移參數量.

圖7 軌跡跟蹤仿真效果對比Fig.7 Comparison of simulation results of trajectory

進一步以絕對誤差積分(integral absolute error,IAE)作為評價標準,對3種控制器效果進行量化對比分析.IAE定義如下:

由3種控制器的量化對比結果表1所示,相對于自適應控制器與數值加速度控制器,本文所提算法的IAExy分別減小了4 m和6 m,IAEθ分別減小了118 rad和391 rad,控制性能有顯著提高,尤其是相對于數值加速度控制增加了重心偏移參數估計以后跟蹤效果明顯增加,驗證所提控制算法有效性.

表1 仿真環境下控制器性能對比Table 1 Comparison of controller performance in simulation environment

5 實驗與結果

為進一步考察所提控制算法在面向步行訓練機器人實際應用中的有效性,本節將在實驗室環境內對機器人進行路徑跟蹤實驗.實驗環境如圖8所示,采用超寬帶(ultra wide band,UWB)定位技術進行位置檢測,為避免遮擋問題,標簽芯片由支架支撐并放置在機器人正上方,檢測的位置數據發送給電腦,并基于Python進行控制力計算并以頻率為25 Hz信號發送給機器人.機器人跟蹤目標路徑為C型,負載為1名23歲健康男性模擬腿部殘疾者借助機器人行走,其在行走中對機器人的扶板施加較大負重(包括推力和壓力),會顯著引起重心位置變化且實時變化.

圖8 實驗環境圖Fig.8 Experimental environment diagram

5.1 訓練對比模型

本小節基于LSTM神經網絡基于實驗數據建立機器人的對比模型.具體過程為:先使機器人在空載條件下以設定軌跡進行移動,并實時記錄所發送控制輸入與所測當前位置,共采集500個時間序列共3500個點作為訓練數據;然后基于LSTM神經網絡,以所發送控制力為輸入,傳感器所測相應位置數據為輸出,對所建立對比模型(8)進行訓練,其神經網絡輸入輸出數據如圖9–10所示.

圖9 各個輪子上的驅動力Fig.9 Driving force on each wheel

圖10 機器人在x軸、y軸方向上位置的變化量Fig.10 Variation of robot position in x-axis and y-axis directions

5.2 軌跡跟蹤實驗及結果

C型路徑的目標軌跡如下所示:

采用文獻[22]中數值加速度控制器以及文獻[17]中的模型預測控制器和本文所設計的控制器進行對比實驗,實驗結果如圖11所示.文獻[17]控制器的參數如下:R=diag{0.2,0.1,0.1,0.2},Q=diag{1.3,1.5,0.2}.文獻[22]及本文所用數值加速度控制器參數如下:kpx=1.2,kpy=1.2,kpθ=1.2,kdx=8,kdy=8,kdθ=8.本文重心偏移估計參數及初始值如下:λ=0.3,=0,=0.

圖11 實驗機器人跟蹤結果圖Fig.11 Experimental robot tracking result diagram

由圖11可以看出,本文所設計的控制器相對于文獻[22]中所設計的數值加速度控制及文獻[17]中所設計的模型預測控制相比,其控制精度更高.為進一步分析所提算法的效果,本文對圖11所示跟蹤結果進行量化分析,如表2 所示,MAD 表示其平均絕對誤差(mean absolute deviation),MAX表示最大值.由表2可看出,本文所提算法比數值加速度控制器的跟蹤誤差減小0.58 m,比文獻[17]采用的模型預測控制跟蹤誤差減小0.1027 m.

表2 實驗環境下控制器性能對比Table 2 Comparison of controller performance under experimental environment

綜上所述,針對重心偏移干擾下全向移動機器人軌跡跟蹤控制問題,含有重心偏移參數估計及預測的數值加速度控制器具有良好的魯棒性,機器人可以較為快速精確地跟蹤期望軌跡.實驗結果驗證了含有重心偏移補償控制算法的有效性.

6 結論

本文針對全方位移動機器人運動過程中重心偏移不確定性問題,提出了基于動力學模型的移動機器人重心偏移參數估計及預測的數值加速度軌跡跟蹤控制方法.推導了含有重心偏移干擾的動力學模型,提出了一種新型參數估計法對重心偏移量進行實時估計并通過ZNN求逆算法進行預測,然后通過數值加速度控制器對其進行實時控制.仿真及實驗結果表明,在重心偏移參數未知且變化的情況下,該方法能夠通過當前位置估計出參數真實值,使得位置偏差收斂到參考軌跡,且相對于數值加速度與自適應控制器而言跟蹤精度有顯著增加.