何德峰，徐晨輝，朱 威，鄭雅羽

(浙江工業(yè)大學(xué) 信息工程學(xué)院，浙江 杭州 310023)

近年來(lái)，在日常文化發(fā)展過(guò)程中，舞臺(tái)演藝文化逐漸占據(jù)重要地位。一場(chǎng)文化演出的成功因素已經(jīng)不再局限于劇本、演員、獨(dú)白、服裝與道具等傳統(tǒng)元素[1]，現(xiàn)代演藝更多地將舞臺(tái)設(shè)備融入到演出的各個(gè)環(huán)節(jié)，由此對(duì)舞臺(tái)電子技術(shù)的發(fā)展提出了更高的要求，并催生了大量的相關(guān)研究[2-5]，其中移動(dòng)舞臺(tái)機(jī)器人是呈現(xiàn)高品質(zhì)舞臺(tái)演出藝術(shù)的重要道具之一[6-7]。為了配合劇情演繹，移動(dòng)舞臺(tái)機(jī)器人需要在保證設(shè)備和人員安全的情況下平穩(wěn)且準(zhǔn)確地到達(dá)指定位置，然后跟蹤預(yù)先規(guī)劃的軌跡路線進(jìn)行無(wú)偏運(yùn)動(dòng)，以此實(shí)現(xiàn)演出場(chǎng)景的動(dòng)態(tài)變化。在此過(guò)程中，移動(dòng)舞臺(tái)機(jī)器人通常采用各類(lèi)傳感器測(cè)量狀態(tài)數(shù)據(jù)以實(shí)現(xiàn)反饋控制。然而復(fù)雜多變的舞臺(tái)環(huán)境容易降低數(shù)據(jù)采集的準(zhǔn)確性，進(jìn)而影響舞臺(tái)機(jī)器人的正常運(yùn)行，甚至引發(fā)安全事故。因此，在演出過(guò)程中需要對(duì)舞臺(tái)機(jī)器人的運(yùn)行狀態(tài)進(jìn)行實(shí)時(shí)監(jiān)控，以便及時(shí)發(fā)現(xiàn)并處理各類(lèi)安全隱患。由于通信噪聲會(huì)加劇測(cè)量數(shù)據(jù)的不確定性，監(jiān)控端需對(duì)接收的數(shù)據(jù)進(jìn)行處理，從而獲得舞臺(tái)機(jī)器人準(zhǔn)確的狀態(tài)信息，通?？刹捎脿顟B(tài)估計(jì)的方式，即根據(jù)接收的系統(tǒng)輸出數(shù)據(jù)和相關(guān)的參數(shù)信息估計(jì)出系統(tǒng)狀態(tài)。常見(jiàn)的狀態(tài)估計(jì)算法主要有Kalman濾波[8-13]、滾動(dòng)時(shí)域估計(jì)[14-23]等，其中滾動(dòng)時(shí)域估計(jì)由于其具有處理范數(shù)有界擾動(dòng)的復(fù)雜非線性和受約束系統(tǒng)的能力而受到國(guó)內(nèi)外學(xué)者的廣泛關(guān)注。滾動(dòng)時(shí)域估計(jì)是一種基于優(yōu)化的狀態(tài)估計(jì)算法[19]，通過(guò)最小化在固定大小的滑動(dòng)窗口上定義的代價(jià)函數(shù)來(lái)獲得最優(yōu)狀態(tài)估計(jì)。代價(jià)函數(shù)通常包括總結(jié)了過(guò)去數(shù)據(jù)的到達(dá)代價(jià)項(xiàng)，以及利用輸出作為估計(jì)依據(jù)的輸出誤差項(xiàng)。滾動(dòng)時(shí)域估計(jì)的概念源于所謂的全信息估計(jì)(FIE)，其目的是根據(jù)從初始時(shí)刻到當(dāng)前時(shí)刻的所有過(guò)去測(cè)量值來(lái)生成狀態(tài)估計(jì)，這種估計(jì)算法被稱(chēng)為計(jì)算難解[19]。為了避免由此帶來(lái)的計(jì)算負(fù)擔(dān)，將全信息用固定長(zhǎng)度的時(shí)域窗口內(nèi)的信息來(lái)近似，從而設(shè)計(jì)了滾動(dòng)時(shí)域估計(jì)算法[14]。

目前，滾動(dòng)時(shí)域估計(jì)在移動(dòng)機(jī)器人領(lǐng)域已有大量的應(yīng)用[17,21-23]，如文獻(xiàn)[17]研究了針對(duì)移動(dòng)機(jī)器人多速率采樣問(wèn)題的滾動(dòng)時(shí)域估計(jì)算法，將未采樣的傳感器輸出視為丟包，并用二進(jìn)制切換序列進(jìn)行建模。采用預(yù)測(cè)器對(duì)測(cè)量輸出進(jìn)行補(bǔ)償，進(jìn)而通過(guò)求解正則化最小二乘問(wèn)題得到最優(yōu)估計(jì)。文獻(xiàn)[21]考慮多個(gè)移動(dòng)機(jī)器人對(duì)一個(gè)動(dòng)態(tài)目標(biāo)進(jìn)行軌跡跟蹤的場(chǎng)景，針對(duì)目標(biāo)具有的噪聲和狀態(tài)約束，提出了一種基于滾動(dòng)時(shí)域估計(jì)的分布式估計(jì)算法，該算法中的協(xié)商一致性過(guò)程保證了所有移動(dòng)機(jī)器人實(shí)現(xiàn)對(duì)運(yùn)動(dòng)目標(biāo)的一致性估計(jì)。文獻(xiàn)[22]研究了分布式多機(jī)器人的同步定位與地圖構(gòu)建(SLAM)問(wèn)題，基于機(jī)器人與鄰居機(jī)器人共享測(cè)量值和狀態(tài)估計(jì)，提出了一種分布式滾動(dòng)時(shí)域估計(jì)算法，并采用C/GMRES方法對(duì)算法進(jìn)行優(yōu)化，以提高計(jì)算效率。針對(duì)移動(dòng)機(jī)器人中出現(xiàn)的狀態(tài)估計(jì)和跟蹤控制問(wèn)題，文獻(xiàn)[23]提出了一種基于優(yōu)化的解決方案，其中考慮利用滾動(dòng)時(shí)域估計(jì)算法求解估計(jì)問(wèn)題，利用非線性模型預(yù)測(cè)控制求解跟蹤控制問(wèn)題，并將所提出的方案與其它兩種常用的狀態(tài)估計(jì)和控制技術(shù)進(jìn)行了比較，驗(yàn)證了方案的良好性能。盡管如此，針對(duì)移動(dòng)舞臺(tái)機(jī)器人的運(yùn)行狀態(tài)估計(jì)結(jié)果鮮有報(bào)道。筆者慮及移動(dòng)舞臺(tái)機(jī)器人運(yùn)行過(guò)程中的安全監(jiān)控問(wèn)題，提出一種基于滾動(dòng)時(shí)域優(yōu)化的移動(dòng)舞臺(tái)機(jī)器人位姿估計(jì)算法。首先，結(jié)合移動(dòng)舞臺(tái)機(jī)器人的運(yùn)動(dòng)學(xué)模型和預(yù)設(shè)的參考位姿，采用PD控制，建立移動(dòng)舞臺(tái)機(jī)器人閉環(huán)誤差系統(tǒng)模型；其次，采用滾動(dòng)時(shí)域估計(jì)算法對(duì)舞臺(tái)機(jī)器人跟蹤誤差進(jìn)行估計(jì)，進(jìn)而結(jié)合已知的參考位姿計(jì)算出舞臺(tái)機(jī)器人的估計(jì)位姿，以此獲得移動(dòng)舞臺(tái)機(jī)器人的準(zhǔn)確位姿；最后，結(jié)合移動(dòng)舞臺(tái)機(jī)器人進(jìn)行曲線運(yùn)動(dòng)的場(chǎng)景來(lái)仿真驗(yàn)證該算法的有效性。

1 移動(dòng)舞臺(tái)機(jī)器人閉環(huán)跟蹤系統(tǒng)模型

考慮如圖1所示的移動(dòng)舞臺(tái)機(jī)器人，以全局坐標(biāo)來(lái)描述移動(dòng)舞臺(tái)機(jī)器人的行進(jìn)路線和位姿。其中，以XOY表示真實(shí)環(huán)境下的全局坐標(biāo)系，機(jī)器人在該坐標(biāo)系中的位姿完全由向量(x,y,θ)指定，其中(x,y)為機(jī)器人中心點(diǎn)的橫縱坐標(biāo)，θ為機(jī)器人的旋轉(zhuǎn)角度。

圖1 移動(dòng)舞臺(tái)機(jī)器人位姿示意圖

對(duì)于圖1所示的移動(dòng)舞臺(tái)機(jī)器人，建立其運(yùn)動(dòng)學(xué)模型[6]為

(1)

式中：v，ω分別為機(jī)器人的移動(dòng)線速度和旋轉(zhuǎn)角速度，同時(shí)也是機(jī)器人的控制輸入。假設(shè)機(jī)器人在側(cè)向上不打滑，即不能沿著驅(qū)動(dòng)輪軸線的方向運(yùn)動(dòng)。考慮移動(dòng)舞臺(tái)機(jī)器人的軌跡跟蹤控制問(wèn)題，設(shè)計(jì)反饋控制器控制v和ω，使得機(jī)器人從任意初始位姿出發(fā)都能夠準(zhǔn)確跟蹤已知的參考位姿，其表達(dá)式為

(2)

(3)

(4)

(5)

將式(3)代入式(5)，可得

(6)

式(6)等價(jià)于

(7)

進(jìn)而可得

(8)

將式(8)代入式(4)，可得閉環(huán)誤差系統(tǒng)模型為

(9)

假設(shè)KP和KD選擇合理，使得PD控制器具有良好的控制性能，進(jìn)而保證閉環(huán)誤差系統(tǒng)式(9)能快速收斂到穩(wěn)態(tài)點(diǎn)，即零點(diǎn)。

2 移動(dòng)舞臺(tái)機(jī)器人的位姿估計(jì)

在實(shí)際運(yùn)行過(guò)程中，移動(dòng)舞臺(tái)機(jī)器人自身配備的傳感器實(shí)時(shí)測(cè)量運(yùn)行狀態(tài)，并將數(shù)據(jù)發(fā)送給監(jiān)控端。然而舞臺(tái)上煙霧、焰火和燈光等設(shè)備的使用容易加劇移動(dòng)舞臺(tái)機(jī)器人實(shí)際運(yùn)行環(huán)境的復(fù)雜性，進(jìn)而干擾機(jī)器人的正常運(yùn)行和傳感器的數(shù)據(jù)采集，使得測(cè)量得到的數(shù)據(jù)往往不能準(zhǔn)確反映系統(tǒng)的真實(shí)狀態(tài)，因此監(jiān)控端需通過(guò)狀態(tài)估計(jì)器，利用接收的數(shù)據(jù)和已知的先驗(yàn)信息(如機(jī)器人模型參數(shù)、噪聲統(tǒng)計(jì)特性等)實(shí)時(shí)估計(jì)機(jī)器人的狀態(tài)。取采樣周期為T(mén)，對(duì)模型式(9)進(jìn)行離散化處理，可得閉環(huán)誤差系統(tǒng)離散模型為

(10)

考慮采用高斯噪聲來(lái)近似復(fù)雜環(huán)境對(duì)機(jī)器人的影響，基于模型式(10)可得干擾下的移動(dòng)舞臺(tái)機(jī)器人閉環(huán)誤差系統(tǒng)離散模型為

(11)

(12)

(13)

式中：Q為已知的過(guò)程噪聲協(xié)方差矩陣；R為已知的測(cè)量噪聲協(xié)方差矩陣；E[·]為數(shù)學(xué)期望。

考慮到移動(dòng)舞臺(tái)機(jī)器人在每個(gè)時(shí)刻的參考位姿可根據(jù)模型(式2)單獨(dú)計(jì)算，結(jié)合移動(dòng)舞臺(tái)機(jī)器人的閉環(huán)誤差系統(tǒng)模型，機(jī)器人運(yùn)行位姿的估計(jì)問(wèn)題可轉(zhuǎn)化為對(duì)系統(tǒng)(式11)狀態(tài)的實(shí)時(shí)估計(jì)問(wèn)題，即每個(gè)時(shí)刻先估計(jì)系統(tǒng)(式11)的狀態(tài)，再利用估計(jì)的跟蹤誤差和已知的參考位姿估計(jì)出機(jī)器人在當(dāng)前時(shí)刻的位姿。對(duì)系統(tǒng)模型(式11，12)進(jìn)行簡(jiǎn)化，可得

Xk+1=AXk+εk

yk=CXk+ηk

(14)

針對(duì)系統(tǒng)式(14)，采用滾動(dòng)時(shí)域估計(jì)算法對(duì)其進(jìn)行估計(jì)，該算法在一個(gè)長(zhǎng)度有限的時(shí)域窗口內(nèi)，利用系統(tǒng)輸出值和預(yù)先給定的狀態(tài)先驗(yàn)值來(lái)估計(jì)系統(tǒng)狀態(tài)，具體描述如下：

(15)

(16)

(17)

(18)

(19)

3 仿真與結(jié)果分析

為檢驗(yàn)滾動(dòng)時(shí)域估計(jì)算法對(duì)移動(dòng)舞臺(tái)機(jī)器人位姿的估計(jì)效果，給出一個(gè)移動(dòng)舞臺(tái)機(jī)器人進(jìn)行簡(jiǎn)單曲線運(yùn)動(dòng)的仿真例子進(jìn)行驗(yàn)證，同時(shí)以估計(jì)結(jié)果的均方誤差為標(biāo)準(zhǔn)，對(duì)比算法在不同輸出矩陣和噪聲強(qiáng)度下的估計(jì)性能。仿真參數(shù)如表1所示，其中量測(cè)噪聲和過(guò)程噪聲由Matlab函數(shù)randn生成，I表示具有合適維度的單位矩陣。機(jī)器人的參考位姿設(shè)定為從坐標(biāo)系的任意位置出發(fā)，以0.5 m/s的線速度和0.3 deg/s的角速度運(yùn)行而生成的位姿。在表1所示參數(shù)的設(shè)定下，滾動(dòng)時(shí)域估計(jì)算法對(duì)移動(dòng)舞臺(tái)機(jī)器人位姿的估計(jì)結(jié)果如圖2～5所示。

表1 仿真參數(shù)表

圖2 機(jī)器人軌跡估計(jì)

圖3 機(jī)器人橫向位置誤差估計(jì)

圖4 機(jī)器人縱向位置誤差估計(jì)

圖5 機(jī)器人旋轉(zhuǎn)角誤差估計(jì)

由圖2可知：機(jī)器人的參考軌跡呈橢圓形，軌跡的中心點(diǎn)坐標(biāo)在(1.2, 3.8)附近。機(jī)器人初始位置的坐標(biāo)大致為(0, 1)，PD控制器能使機(jī)器人在初始位置偏離參考軌跡的情況下快速跟蹤到預(yù)定的參考軌跡。由于噪聲的影響，機(jī)器人的實(shí)際軌跡會(huì)圍繞參考軌跡出現(xiàn)細(xì)微波動(dòng)。同時(shí)，滾動(dòng)時(shí)域估計(jì)算法能快速收斂，使估計(jì)的軌跡能準(zhǔn)確跟蹤真實(shí)的軌跡。

圖3～5展示了估計(jì)算法對(duì)跟蹤誤差的估計(jì)情況，由圖3～5可知：跟蹤誤差的收斂時(shí)間大致為5 s，之后在噪聲的影響下，跟蹤誤差圍繞零點(diǎn)出現(xiàn)波動(dòng)；同時(shí)，估計(jì)算法能較好地估計(jì)出跟蹤誤差，估計(jì)誤差的收斂時(shí)間大致為5 s。

表2為滾動(dòng)時(shí)域估計(jì)算法在不同強(qiáng)度噪聲下對(duì)移動(dòng)舞臺(tái)機(jī)器人跟蹤誤差的均方估計(jì)誤差對(duì)比情況。由表2可知：隨著測(cè)量噪聲的協(xié)方差逐漸增大，機(jī)器人跟蹤誤差3個(gè)分量的均方估計(jì)誤差也隨之增大，原因是測(cè)量噪聲強(qiáng)度的增大降低了系統(tǒng)測(cè)量輸出的準(zhǔn)確性，進(jìn)而增加了估計(jì)的難度?？傊?，與噪聲強(qiáng)度的增加幅度相比，均方估計(jì)誤差的增加幅度明顯更小，這表明滾動(dòng)時(shí)域估計(jì)算法對(duì)噪聲具有一定的抑制能力。

表2 不同噪聲強(qiáng)度下的均方估計(jì)誤差對(duì)比

表3為滾動(dòng)時(shí)域估計(jì)算法在不同系統(tǒng)輸出矩陣下對(duì)移動(dòng)舞臺(tái)機(jī)器人跟蹤誤差的均方估計(jì)誤差對(duì)比情況。由表3可知：不同的系統(tǒng)輸出矩陣會(huì)使估計(jì)算法產(chǎn)生不同的估計(jì)效果。就跟蹤誤差的單個(gè)分量ye而言，若系統(tǒng)輸出數(shù)據(jù)中包含與ye相關(guān)的信息，則該信息與ye的相關(guān)度越高，估計(jì)算法對(duì)ye的均方估計(jì)誤差越小，估計(jì)效果越好，該結(jié)果與估計(jì)算法的原理相符。對(duì)于算法而言，系統(tǒng)輸出數(shù)據(jù)是算法獲得系統(tǒng)狀態(tài)信息的直接來(lái)源，因此輸出數(shù)據(jù)中與狀態(tài)分量相關(guān)的信息越準(zhǔn)確，算法的估計(jì)效果就越好。

表3 不同輸出矩陣下的均方估計(jì)誤差對(duì)比

4 結(jié) 論

提出了一種基于滾動(dòng)時(shí)域優(yōu)化的移動(dòng)舞臺(tái)機(jī)器人位姿估計(jì)算法來(lái)處理移動(dòng)舞臺(tái)機(jī)器人的位姿監(jiān)控問(wèn)題。首先，基于移動(dòng)舞臺(tái)機(jī)器人的運(yùn)動(dòng)學(xué)模型和預(yù)設(shè)的參考位姿模型，提出了移動(dòng)舞臺(tái)機(jī)器人的開(kāi)環(huán)誤差動(dòng)力學(xué)系統(tǒng)模型，考慮移動(dòng)舞臺(tái)機(jī)器人的軌跡跟蹤控制問(wèn)題，采用PD控制，建立了舞臺(tái)機(jī)器人的閉環(huán)誤差動(dòng)力學(xué)系統(tǒng)模型。其次，用高斯噪聲模擬復(fù)雜環(huán)境的影響，采用滾動(dòng)時(shí)域估計(jì)算法對(duì)移動(dòng)舞臺(tái)機(jī)器人的跟蹤誤差進(jìn)行估計(jì)，結(jié)合已知的參考位姿，反推出移動(dòng)舞臺(tái)機(jī)器人的估計(jì)位姿，以此獲得移動(dòng)舞臺(tái)機(jī)器人在復(fù)雜環(huán)境下的準(zhǔn)確位姿。最后，以移動(dòng)舞臺(tái)機(jī)器人進(jìn)行簡(jiǎn)單的曲線運(yùn)動(dòng)為例進(jìn)行仿真。結(jié)果表明：PD控制能使機(jī)器人準(zhǔn)確跟蹤預(yù)設(shè)的參考位姿，同時(shí)滾動(dòng)時(shí)域估計(jì)算法能在數(shù)據(jù)受到噪聲污染的情況下有效估計(jì)出機(jī)器人的跟蹤誤差，進(jìn)而估計(jì)出機(jī)器人的真實(shí)位姿。