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

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

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

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

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

考慮如圖1所示的移動舞臺機(jī)器人，以全局坐標(biāo)來描述移動舞臺機(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 移動舞臺機(jī)器人位姿示意圖

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

(1)

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

(2)

(3)

(4)

(5)

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

(6)

式(6)等價于

(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 移動舞臺機(jī)器人的位姿估計(jì)

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

(10)

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

(11)

(12)

(13)

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

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

Xk+1=AXk+εk

yk=CXk+ηk

(14)

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

(15)

(16)

(17)

(18)

(19)

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

為檢驗(yàn)滾動時域估計(jì)算法對移動舞臺機(jī)器人位姿的估計(jì)效果，給出一個移動舞臺機(jī)器人進(jìn)行簡單曲線運(yùn)動的仿真例子進(jìn)行驗(yàn)證，同時以估計(jì)結(jié)果的均方誤差為標(biāo)準(zhǔn)，對比算法在不同輸出矩陣和噪聲強(qiáng)度下的估計(jì)性能。仿真參數(shù)如表1所示，其中量測噪聲和過程噪聲由Matlab函數(shù)randn生成，I表示具有合適維度的單位矩陣。機(jī)器人的參考位姿設(shè)定為從坐標(biāo)系的任意位置出發(fā)，以0.5 m/s的線速度和0.3 deg/s的角速度運(yùn)行而生成的位姿。在表1所示參數(shù)的設(shè)定下，滾動時域估計(jì)算法對移動舞臺機(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í)際軌跡會圍繞參考軌跡出現(xiàn)細(xì)微波動。同時，滾動時域估計(jì)算法能快速收斂，使估計(jì)的軌跡能準(zhǔn)確跟蹤真實(shí)的軌跡。

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

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

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

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

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

4 結(jié) 論

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