基于粒子濾波的欠驅(qū)動移動機器人多目標(biāo)點跟蹤控制

君萌萌，張 敏，姜志偉，任鵬舟，左 磊

(長安大學(xué) 電子與控制工程學(xué)院，西安 710064)

0 引言

近年來，移動機器人目標(biāo)點跟蹤控制是機器人控制領(lǐng)域內(nèi)的熱點問題[1-3]，也是多目標(biāo)跟蹤、軌跡跟蹤以及移動目標(biāo)跟蹤等諸多領(lǐng)域的研究基礎(chǔ)[4-6]。目標(biāo)點跟蹤控制的實質(zhì)是根據(jù)移動機器人的運動學(xué)特性，設(shè)計相應(yīng)的控制方法，使得機器人能夠從任意位置逐漸收斂到目標(biāo)點附近。鑒于上述目標(biāo)點跟蹤控制的重要性，大量學(xué)者針對該問題展開了深入的研究。例如，文獻[7]提出一種飽和反饋控制器來解決移動機器人的跟蹤問題。文獻[8]采用模型預(yù)測控制技術(shù)開發(fā)控制器，研究具有耦合輸入約束的目標(biāo)點跟蹤控制。文獻[9]提出具有一組不等式約束的控制方案來滿足速度和扭矩約束。此外，文獻[10]研究基于事件觸發(fā)機制的跟蹤控制方法。文獻[11]研究避免碰撞的跟蹤控制方法。文獻[12]研究了保持安全距離的目標(biāo)跟蹤系統(tǒng)。文獻[13]設(shè)計了模糊控制器實現(xiàn)軌跡跟蹤控制。針對跟蹤精度問題，文獻[14]提出基于模型預(yù)測控制的軌跡跟蹤系統(tǒng)，其中線性模型預(yù)測控制器可有效提高優(yōu)化算法的運算效率。盡管現(xiàn)存文獻已對移動機器人目標(biāo)點跟蹤控制進行了深入研究，但在實際應(yīng)用中仍存在許多問題。例如，傳統(tǒng)的目標(biāo)跟蹤控制方法跟蹤精度較低，難以滿足較高精度的跟蹤控制需求或會導(dǎo)致跟蹤控制系統(tǒng)出現(xiàn)奇異現(xiàn)象。因此，有必要深入研究移動機器人高精度跟蹤控制。

由于目標(biāo)跟蹤精度控制問題中位置信息尤為重要，然而在實際中位置信息受傳感器采樣噪聲影響較大，粒子濾波方法作為一種成熟的濾波算法，是使用一系列帶權(quán)值的空間隨機采樣粒子來逼近機器人的真實位置[15]。由此可知，粒子濾波獨立于系統(tǒng)模型，不受各種誤差或噪聲的約束，可適用于任何環(huán)境下的狀態(tài)測量與估計。例如，利用粒子濾波方法實現(xiàn)有效跟蹤預(yù)測[16-18]和目標(biāo)定位[19-20]。盡管粒子濾波在定位精度方面取得了一定的研究成果，但在移動機器人領(lǐng)域內(nèi)的應(yīng)用范圍有限，仍需開展進一步研究。

根據(jù)上述現(xiàn)狀分析，本文利用粒子濾波算法，提出了一種面向欠驅(qū)動移動機器人的高精度目標(biāo)點跟蹤控制方法。其主要貢獻為：在考慮移動機器人采樣位置噪聲的情況下，利用粒子濾波方法得到精準(zhǔn)可靠的移動機器人位置信息。在此基礎(chǔ)上，設(shè)計基于反饋控制的多目標(biāo)點跟蹤控制方法，并改進了其中控制參數(shù)的約束條件，使得欠驅(qū)動移動機器人能夠快速、精準(zhǔn)地到達目標(biāo)點。最后，通過理論分析與數(shù)值仿真相結(jié)合的方式，驗證了所提算法的穩(wěn)定性與可行性。

1 車輛模型

考慮如下差分型欠驅(qū)動移動機器人，其運動學(xué)模型如下所示:

vko=(vl+vr)/2

ωko=(vl-vr)/D

(1)

其中:vl和vr分別代表左、右車輪的速度，D是兩車輪之間的距離，vko和ωko分別表示前向速度和角速度；θk表示航向角；P=[px,py]T表示移動機器人的位置協(xié)調(diào)中心。

假設(shè)目標(biāo)點序列為Wk∈R2,k={1,…n}，其中n為目標(biāo)點的個數(shù)。移動機器人與目標(biāo)點之間的位置關(guān)系如圖1所示。從圖1可知，移動機器人與目標(biāo)點之間的位置關(guān)系可數(shù)學(xué)描述為：

(2)

其中:dWko是移動機器人到第k個目標(biāo)點的距離，αko是移動機器人的轉(zhuǎn)向角，σko是移動機器人車身固定框架到全局框架的方向。Wk=[Wkx,Wky]是第k個目標(biāo)點的位置。

圖1 移動機器人與目標(biāo)點之間的位置關(guān)系

根據(jù)移動機器人的運動學(xué)特性及其與目標(biāo)點的位置關(guān)系，基于移動機器人位置的極坐標(biāo)運動學(xué)模型為：

(3)

需要注意的是，式(3)中的分母表示移動機器人到目標(biāo)點的距離。當(dāng)移動機器人到達目標(biāo)點時，跟蹤系統(tǒng)達到奇異點。為解決此問題，傳統(tǒng)的研究方案使用一個半徑為R的圓代替目標(biāo)點。這種跟蹤方式的精度往往取決于圓半徑R。當(dāng)需要達到高精度的跟蹤性能時，傳統(tǒng)的跟蹤方法可能無法滿足精度要求或會出現(xiàn)奇異現(xiàn)象。因此，本文將根據(jù)欠驅(qū)動移動機器人的運動學(xué)模型，提出一種新的跟蹤控制方案來提高移動機器人到目標(biāo)點的跟蹤精度。

2 基于粒子濾波的車輛位置信息處理

本文考慮移動機器人的采樣噪聲，使用粒子濾波方法獲得移動機器人的量測位置狀態(tài)。假設(shè)k時刻移動機器人離散系統(tǒng)的狀態(tài)方程和量測方程[21]為：

x(k)=f[x(k-1),l(k-1)]

z(k)=g[x(k),s(k)]

(4)

其中:x=[px,py]T為系統(tǒng)狀態(tài)，z為觀測數(shù)據(jù)(傳感器獲得的位置數(shù)據(jù))，f(·),g(·)分別是狀態(tài)轉(zhuǎn)移函數(shù)和觀測函數(shù)，l,s分別是系統(tǒng)狀態(tài)噪聲和量測噪聲。設(shè)z1:k={z1,z2,…zk}為1～k時刻所有觀測集合。

本文系統(tǒng)選取的狀態(tài)轉(zhuǎn)移函數(shù)規(guī)定在前后兩幀觀測時間內(nèi)前向速度v和角速度ω保持不變。選取x=[px,py]T為狀態(tài)變量，則式(4)中移動機器人系統(tǒng)的狀態(tài)轉(zhuǎn)移函數(shù)f[x(k-1)]為：

(5)

對權(quán)重進行歸一化處理，可得：

(6)

5)狀態(tài)輸出。由于x(k)的最小方差估計就是x(k)的條件期望，則k時刻移動機器人系統(tǒng)的狀態(tài)輸出為：

(7)

在該算法中，k=0時刻初始化采樣粒子，生成權(quán)值相等的均勻分布粒子集合。移動機器人運動過程中，每個粒子按照給定的狀態(tài)轉(zhuǎn)移函數(shù)進行狀態(tài)更新，之后根據(jù)傳感器測量值計算全部粒子的權(quán)重，最后加權(quán)權(quán)重就是系統(tǒng)的狀態(tài)輸出。與此同時，當(dāng)有效粒子數(shù)小于閾值Ns時，對粒子集合進行重采樣，淘汰權(quán)值過低的粒子，復(fù)制權(quán)值高的粒子組成新的粒子集,繼續(xù)進行系統(tǒng)狀態(tài)轉(zhuǎn)移傳播，構(gòu)成一個循環(huán)濾波系統(tǒng)。

3 基于反饋控制的多目標(biāo)點跟蹤控制

根據(jù)上述基于粒子濾波的移動機器人量測位置信息，移動機器人與目標(biāo)之間的位置關(guān)系式(2)可表示為：

(8)

在此基礎(chǔ)上，移動機器人跟蹤目標(biāo)的運動學(xué)模型可表述為：

(9)

在此基礎(chǔ)上，設(shè)計如下欠驅(qū)動移動機器人目標(biāo)點跟蹤控制律：

vk=kddWk

ωk=kααk+sgn*kσσk

(10)

其中控制增益kd,kα,kσ滿足以下約束條件:

kd>0

sgn*kσ<0

sgn*kσ+kα-kd>0

(11)

具體地，給出如下移動機器人目標(biāo)點跟蹤控制定理:

定理1：考慮移動機器人目標(biāo)點跟蹤問題，其中移動機器人的運動學(xué)模型由式(1)表述。在考慮測量噪聲的情況下，移動機器人的位置信息可通過式(7)獲得，且移動機器人和目標(biāo)點之間的運動學(xué)模型由式(9)表示。在此基礎(chǔ)上，利用式(10)中提出的控制律以及式(11)中的控制增益約束條件，移動機器人將從任意初始位置漸近收斂到目標(biāo)點。

根據(jù)式(9)和(10)，可得移動機器人目標(biāo)跟蹤系統(tǒng)如下：

(12)

然后，考慮下面的 Lyapunov 函數(shù):

υ1=(kσσk-kσαk)2+2sgn*kσkd(cosαk-1)

因為cosαk-1≤0,?αk∈[0,2π]，調(diào)用式(11)參數(shù)條件:kd>0和sgn*kσ<0,得到:

sgn*kσkd(cosαk-1)≥0

因為(kσσk-kσαk)2≥0，υ1滿足υ1≥0。

當(dāng)σk=0,αk=0時，υ1=0，那么σk=0,αk=0是系統(tǒng)式(12)的平衡狀態(tài)，υ1滿足李亞普諾夫穩(wěn)定性定理的正定性。然后對υ1求導(dǎo)，得到:

(13)

將式(12)代入式(13)，可得:

對αk∈[-π,π]，有sinαkαk≥0和sgn*kσkd<0。此外sinαk/αk可表示為:

根據(jù)式(11)參數(shù)條件sgn*kσ+kα-kd>0和kd>0，有:

接下來證明式(12)在其平衡點αk=0的局部穩(wěn)定性。在這個平衡點附近，將式(12)線性化如下：

(14)

用G表示式(14)的變換矩陣。G的特征方程為：

detG=(λ+kd)[(sgn*kσ+kα-kd)λ+

λ2-sgn*kσkd]

(15)

其中:λ是矩陣G的特征值。

假設(shè)線性系統(tǒng)式(14)穩(wěn)定,則detG(λ)=0,G的所有特征值必包含實負(fù)部分。回顧式(11)中的參數(shù)約束，式(15)中的特征值為：

λ1=-kd<0

λ2+λ3=-(sgn*kσ+kα-kd)/2<0

λ2λ3=-sgn*kσkd>0

因此,有real(λ1)<0,real(λ2)<0,real(λ3)<0。這證明了所提跟蹤系統(tǒng)的局部穩(wěn)定性。證畢。

在上述證明中，詳細(xì)分析了所提跟蹤系統(tǒng)的全局穩(wěn)定性和局部穩(wěn)定性。通常，全局穩(wěn)定性足以保證跟蹤系統(tǒng)的可行性。然而,在全局穩(wěn)定性部分只能找到參數(shù)條件sgn*kσ+kα-kd的作用。局部穩(wěn)定性部分給出了條件kd>0和條件sgn*kσ<0的作用機理。因此，有必要對定理1中的全局穩(wěn)定性和局部穩(wěn)定性詳細(xì)分析。

本文提供的控制增益不是從變換矩陣G的特征值推導(dǎo)出來的。事實上，通過變換矩陣的特征值即可得到控制增益。然而，這種方法只適用于線性系統(tǒng)。本文提出的跟蹤系統(tǒng)式(12)是非線性系統(tǒng)。在非線性系統(tǒng)中，通過變換矩陣直接獲得控制增益是比較困難的。因此，本文基于反饋控制得到了所提跟蹤控制律中相關(guān)控制增益的范圍。

此外，本文還提出一組輔助參數(shù)條件提高系統(tǒng)跟蹤性能：

(16)

調(diào)用2sgn*kσk+kαk-(2/π)kdk>0,有：

(17)

然后，移動機器人繼續(xù)向目標(biāo)點移動，這提高了跟蹤效率。

(18)

然后，移動機器人將轉(zhuǎn)向它的目標(biāo)點，并繼續(xù)向它前進。

輔助條件2sgn*kσ+kα-kd可能包含在參數(shù)條件sgn*kσ+kα-kd。然而,如果只應(yīng)用該控制律sgn*kσ+kα-kd,在跟蹤過程中航向角αk可能會不在[-π/2,π/2]范圍。因此，為了提高跟蹤性能分別提出這兩個條件。

(19)

對于算法中的目標(biāo)躍遷臨界值ε，本文所提出的跟蹤方法可以不受任何限制地進行設(shè)置。然而，使用圓形標(biāo)記目標(biāo)的跟蹤算法，躍遷臨界值必須大于其圓形標(biāo)記的半徑。否則，移動機器人將永遠(yuǎn)不會停止跟蹤它的第一個目標(biāo)點。

注1：與一般的圓形目標(biāo)跟蹤方法相比，本節(jié)提出的跟蹤控制方法可以實現(xiàn)移動機器人對目標(biāo)點的高精度跟蹤。一般的圓形標(biāo)記目標(biāo)跟蹤方法，移動機器人會停在目標(biāo)圓的邊界附近，該算法的跟蹤精度受目標(biāo)圓半徑R的限制。相比之下，本節(jié)提出的控制律可以在保證系統(tǒng)穩(wěn)定性的前提下，使移動機器人盡可能到達目標(biāo)點，具有較高的精度。

注 2：雖然所提跟蹤控制方法在實際應(yīng)用中存在諸多問題，但仍具有一定的優(yōu)越性。首先，采用粒子濾波方法降低測量噪聲，提高了跟蹤方法精度；其次，該控制方法給出了相關(guān)參數(shù)增益的選擇范圍；最后，由于本文所提出的跟蹤控制律本質(zhì)上是反饋控制，因此在實驗中保證了跟蹤系統(tǒng)的穩(wěn)定性和跟蹤性能。

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

本節(jié)將給出根據(jù)所設(shè)計的控制系統(tǒng)做出的仿真結(jié)果，以證明本文所提供的理論方法的可行性和有效性。

步驟1：檢驗粒子濾波的位置信息處理效果。根據(jù)移動機器人的離散狀態(tài)方程(4)，給定系統(tǒng)狀態(tài)轉(zhuǎn)移方程的狀態(tài)噪聲為l=wgn(2,1,10*log10(3.42))，觀測方程的量測噪聲為s=wgn(2,1,10*log10(2.28))，移動機器人的初始位置為(50,10)。粒子濾波算法的仿真結(jié)果如圖2所示。

圖2 真實值與濾波后值的誤差

圖2(a)和圖2(b)分別展示了系統(tǒng)真實值和濾波后的值在x方向、y方向的誤差。從該圖中可以看出初始采樣之后，經(jīng)粒子濾波方法得到的位置與真實位置相差極小。圖中誤差周期性地從某個值變小甚至為零，這是因為在迭代過程中粒子退化達到重采樣條件時，復(fù)制較高權(quán)值粒子，減小權(quán)重誤差。該結(jié)果表明移動機器人采樣信息受噪聲干擾之后，經(jīng)過粒子濾波算法得到了較為精準(zhǔn)的移動機器人狀態(tài)信息。

步驟2：在驗證了粒子濾波的位置信息處理效果后，開始對基于粒子濾波的多目標(biāo)點跟蹤控制方法進行檢驗。

1)考慮含有18個目標(biāo)點的序列，移動機器人的運動學(xué)模型由式(1)描述。

2)根據(jù)式(11)控制律參數(shù)條件給定控制收益，當(dāng)‖αk‖≤π/2時，控制收益為kd=1,kα=20,kσ=-2.5，當(dāng)π/2≤‖αk‖≤π時，選擇控制收益為kd=-1,kα=20,kσ=-2.5。

3)設(shè)定式(19)中的目標(biāo)躍遷的臨界值ε=0.001。

移動機器人多目標(biāo)點跟蹤的仿真結(jié)果如圖3～5所示。

圖3 目標(biāo)點軌跡

在圖3中，三角形點表示給定的目標(biāo)點，從該圖可以明顯地看出，所提出的控制方案驅(qū)動差動型機器人通過給定的目標(biāo)點，最終停在最后的目標(biāo)點。需要注意的是圖3所示的跟蹤軌跡并不嚴(yán)格遵循目標(biāo)點的線。這是因為移動機器人驅(qū)動力不足，只有前向速度和航向角受到控制。例如，當(dāng)移動機器人從初始位置駛向其第一個目標(biāo)時，它被直接驅(qū)動駛向第一個目標(biāo)，而無需考慮第二個目標(biāo)。因此，不能保證移動機器人抵達第一個目標(biāo)的最終狀態(tài)。然后，通過所提的控制律，移動機器人的航向角將調(diào)向第二個目標(biāo)，因此導(dǎo)致圖3中的彎曲軌跡。

圖4(a)和圖4(b)分別展現(xiàn)了移動機器人的前進速度v和轉(zhuǎn)向角速度ω的演變。從該圖可以看出，在t=100 s時，v和ω都收斂到零。從圖4(a)可知，移動機器人在t=100 s到達最終目標(biāo)點，然后在此時停止。而圖4(b)中曲線在t=100 s之后有小幅動是因為移動機器人到達最后目標(biāo)點之后的方向校正。此外，前向速度v和角速度ω周期性地從某個值變?yōu)榱悖@是因為移動機器人會定期更改其跟蹤目標(biāo)，改變前向速度和航向角。

圖4 移動機器人速度和角速度變化曲線

圖5(a)和圖5(b)分別展現(xiàn)了移動機器人到當(dāng)前目標(biāo)的跟蹤誤差(Ecc)、移動機器人到最后目標(biāo)點的跟蹤誤差(Ecf)的演變。首先，在圖5(b)中t=100 s時跟蹤誤差收斂到零可驗證移動機器人停止在其最終目標(biāo)位置。然后，在圖5(a)中，跟蹤誤差周期性地變?yōu)榱憧杀砻饕苿訖C器人到達各個目標(biāo)點。該結(jié)果表明：通過使用所提出的控制律，移動機器人將逐個通過一系列目標(biāo)點，并在最后一個目標(biāo)點停止。

圖5 移動機器人和目標(biāo)點的距離

4)關(guān)于目標(biāo)點跟蹤精度問題，以其中一個目標(biāo)點為例，在目標(biāo)躍遷臨界值ε不同的情況下，移動機器人到目標(biāo)點的跟蹤精度如表1所示。

表1 不同臨界值下跟蹤精度

表1展現(xiàn)了在不同的目標(biāo)躍遷臨界值下，移動機器人到目標(biāo)點的跟蹤精度。由該仿真結(jié)果可知，移動機器人的跟蹤精度總能達到或小于臨界值，而該躍遷臨界值可由讀者自行設(shè)置，進而提高移動機器人的跟蹤精度。

5 結(jié)束語

本文研究了基于反饋控制的欠驅(qū)動移動機器人高精度目標(biāo)點跟蹤控制問題。首先利用粒子濾波方法對移動機器人的位置信息進行降噪處理，從而得到較為精確的量測位置。在此基礎(chǔ)上，結(jié)合反饋控制和欠驅(qū)動移動機器人的運動學(xué)模型，提出了具有一組參數(shù)約束條件的高精度目標(biāo)點跟蹤控制方案，使得移動機器人可從任意位置以較高的精度逐漸收斂到目標(biāo)點，并嚴(yán)格分析了所提跟蹤系統(tǒng)的穩(wěn)定性和高精度的跟蹤性能。最后，通過數(shù)值仿真的方式驗證了本文所提算法的可行性和有效性。