基于激光掃描的機(jī)器人運(yùn)動(dòng)目標(biāo)跟蹤技術(shù)研究

黃 東，陳樂(lè)庚

(1. 桂林信息科技學(xué)院電子工程學(xué)院，廣西 桂林 541004；2. 桂林電子科技大學(xué)電子工程與自動(dòng)化學(xué)院，廣西 桂林 541004)

1 引言

隨著人們對(duì)高質(zhì)量生活的不斷追求，機(jī)器人技術(shù)已經(jīng)從工業(yè)領(lǐng)域擴(kuò)展到醫(yī)療、教育、勘測(cè)等領(lǐng)域。在實(shí)際生活中，移動(dòng)機(jī)器人無(wú)處不在?，F(xiàn)階段，對(duì)機(jī)器人研究不只限于其自身特點(diǎn)，更要對(duì)如何解決生活中實(shí)際問(wèn)題進(jìn)行研究。機(jī)器人由先進(jìn)智能系統(tǒng)組成，要實(shí)現(xiàn)其運(yùn)動(dòng)目標(biāo)的跟蹤，需要了解所處環(huán)境的真實(shí)信息。障礙檢測(cè)是目標(biāo)跟蹤的關(guān)鍵內(nèi)容，在傳統(tǒng)算法中，因?qū)Νh(huán)境的不確定性因素考慮不夠，常常導(dǎo)致跟蹤結(jié)果不準(zhǔn)確。近年來(lái)，激光掃描以測(cè)距準(zhǔn)確度高、實(shí)時(shí)性強(qiáng)等優(yōu)勢(shì)在機(jī)器人移動(dòng)目標(biāo)檢測(cè)領(lǐng)域被廣泛使用。

文獻(xiàn)[1]提出基于感知代理與移動(dòng)控制理論的移動(dòng)目標(biāo)跟蹤方法。首先構(gòu)建機(jī)器人的代理模型與感知目標(biāo)跟蹤模型，并提出分布式運(yùn)動(dòng)目標(biāo)檢測(cè)，該方法由估計(jì)部分與一致性部分構(gòu)成，經(jīng)過(guò)這兩個(gè)階段的處理，目標(biāo)狀態(tài)與協(xié)方差矩陣能夠達(dá)成一致，該方法提高了預(yù)測(cè)能力；確保機(jī)器人可以接近運(yùn)動(dòng)目標(biāo)，避免與障礙物發(fā)生碰撞，實(shí)現(xiàn)對(duì)目標(biāo)的跟蹤，但該方法對(duì)機(jī)器人目標(biāo)運(yùn)動(dòng)速度設(shè)置較快，容易會(huì)出現(xiàn)跟蹤丟失，導(dǎo)致跟蹤精度較低。文獻(xiàn)[2]提出利用圖像特性為反饋的模糊控制策略，對(duì)運(yùn)動(dòng)目標(biāo)進(jìn)行跟蹤。在三軸直角坐標(biāo)系內(nèi)，將機(jī)器人、攝像機(jī)與計(jì)算機(jī)做為基礎(chǔ)，建立無(wú)標(biāo)定視覺(jué)的跟蹤系統(tǒng)，該系統(tǒng)的工作原理為，利用攝像機(jī)對(duì)圖像信息進(jìn)行采集，并經(jīng)過(guò)定位處理，識(shí)別出運(yùn)動(dòng)目標(biāo)與機(jī)器人的距離，并利用模糊控制系統(tǒng)控制機(jī)器人的運(yùn)動(dòng)狀態(tài)及軌跡，實(shí)時(shí)對(duì)圖像信息進(jìn)行反饋，結(jié)合模糊推理規(guī)則，對(duì)系統(tǒng)誤差修正，最終達(dá)到穩(wěn)定跟蹤的目的。但該方法對(duì)目標(biāo)跟蹤的速度較慢，導(dǎo)致跟蹤效率較低。

基于上述方法中存在的問(wèn)題，提出基于激光掃描的機(jī)器人運(yùn)動(dòng)目標(biāo)跟蹤技術(shù)。通過(guò)對(duì)坐標(biāo)系分析建立機(jī)器人運(yùn)動(dòng)模型；再根據(jù)障礙物的狀態(tài)，對(duì)運(yùn)動(dòng)目標(biāo)進(jìn)行檢測(cè)，確定機(jī)器人與目標(biāo)之間的合適距離，利用卡爾曼濾波獲取目標(biāo)運(yùn)動(dòng)矩陣，并對(duì)其進(jìn)行跟蹤。與傳統(tǒng)方法相比本文方法通過(guò)對(duì)運(yùn)動(dòng)目標(biāo)的多種處理，提升了運(yùn)動(dòng)目標(biāo)控制的可靠性，提高了跟蹤精準(zhǔn)度及效率。

2 激光掃描的濾波設(shè)計(jì)

在激光掃描過(guò)程中存在噪聲干擾因素，主要包括跟蹤環(huán)境中光源和激光光源相互干擾，導(dǎo)致像素混合；另一方面，測(cè)量目標(biāo)的發(fā)射率過(guò)小或入射角過(guò)大導(dǎo)致數(shù)據(jù)丟失；由于障礙物的遮擋，產(chǎn)生掃描盲區(qū)；激光掃描線(xiàn)存在間隙，導(dǎo)致部分區(qū)域無(wú)法掃描。在對(duì)機(jī)器人進(jìn)行導(dǎo)航時(shí)，由于上述干擾因素的存在，導(dǎo)致避障檢測(cè)不準(zhǔn)確，所以需要對(duì)激光掃描的數(shù)據(jù)進(jìn)行濾波處理[3，4]。

激光掃描測(cè)距信息主要表現(xiàn)為在全部極坐標(biāo)測(cè)量方向上，鄰近時(shí)間點(diǎn)的測(cè)量數(shù)據(jù)存在相關(guān)性，并且在同一組測(cè)量中，鄰近掃描角度上的測(cè)量信息同樣具有較大相關(guān)性。所以，利用在線(xiàn)滾動(dòng)動(dòng)態(tài)自適應(yīng)濾波去除在避障檢測(cè)中的干擾。采用極坐標(biāo)呈現(xiàn)的激光掃描測(cè)距數(shù)據(jù)(ρi，j，λi，j)，構(gòu)建以ρi，j為中心的3×3的信息分析窗口

(1)

式中，ρi，j代表掃描中心到第j個(gè)障礙物之間的距離，λi，j為第j個(gè)障礙物的掃描位置與主掃描位置的角度值，下標(biāo)i為測(cè)距信息的采樣時(shí)間點(diǎn)，j代表測(cè)距信息在同一在數(shù)據(jù)中的排列順序。因此，在該窗口中存在的9個(gè)數(shù)據(jù)在時(shí)間與空間上具有相關(guān)性，利用下述公式計(jì)算ρi.j和其相鄰的數(shù)據(jù)之差Δρmin

Δρmin=min{|ρi+t，j+s-ρi，j|

(2)

式中，t與s不能同時(shí)等于0，Δρmin為時(shí)間和空間上鄰近數(shù)據(jù)的最小差值，假設(shè)Δρmin>δd，此時(shí)，測(cè)量數(shù)據(jù)ρi，j可以看做測(cè)量噪聲值而不參與障礙檢測(cè)評(píng)估。δd代表激光雷掃描測(cè)距的標(biāo)準(zhǔn)差，在不同距離范圍內(nèi)，激光掃描測(cè)距的信息統(tǒng)計(jì)表，利用DAF可以有效去除脈沖干擾噪聲和混合像素干擾，具體如表1 所示：

表1 不同距離標(biāo)準(zhǔn)差取值統(tǒng)計(jì)表

3 激光掃描下機(jī)器人運(yùn)動(dòng)目標(biāo)跟蹤技術(shù)研究

3.1 機(jī)器人坐標(biāo)系與運(yùn)動(dòng)模型建立

在對(duì)移動(dòng)目標(biāo)進(jìn)行跟蹤時(shí)，常用坐標(biāo)系統(tǒng)分為以下三種：第一種為機(jī)器人自身所在環(huán)境的全局坐標(biāo)系[5]，描述為(XG，YG，ZG)，可以展示機(jī)器人在環(huán)境中位置變化情況；第二種為固定在機(jī)器人上的局部坐標(biāo)系[6]，描述為(XR，YR，ZR)，第三種為通常情況下測(cè)距傳感器反饋給機(jī)器人的數(shù)據(jù)信息構(gòu)成的極坐標(biāo)系，描述為(ρ，θ)，如圖1 所示。

圖1 機(jī)器人坐標(biāo)示意圖

針對(duì)坐標(biāo)位置為[x，y，θ]T的機(jī)器人來(lái)說(shuō)，其運(yùn)動(dòng)學(xué)模型可以描述為

(3)

式中，ω表示機(jī)器人中心點(diǎn)的角速度，v為線(xiàn)速度，則上述公式必須符合非完整性的約束條件，即

(4)

在實(shí)際跟蹤過(guò)程中，機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)受到動(dòng)力約束[7]即

|v|≤vmax；|ω|≤ωmax

(5)

機(jī)器人在跟蹤時(shí)，控制左右輪速度，即

(6)

在上述公式基礎(chǔ)上，導(dǎo)出對(duì)應(yīng)的離散化控制模型，即

(7)

3.2 目標(biāo)檢測(cè)視窗構(gòu)建

當(dāng)激光器與物體之間的差距大于三米時(shí)，數(shù)據(jù)點(diǎn)之間的距離明顯增加，對(duì)獲取目標(biāo)數(shù)據(jù)信息產(chǎn)生不利影響。此時(shí)，只需要取三米之內(nèi)的數(shù)據(jù)作為有效測(cè)量信息。并將存在不確定性因素信息剔除，可以減少計(jì)算機(jī)負(fù)擔(dān)。因此，需要在機(jī)器人前方構(gòu)建具有一定半徑的滾動(dòng)檢測(cè)視窗[8]。

在機(jī)器人前方擴(kuò)展一個(gè)半徑為R的半圓檢測(cè)窗口，α和β分別代表此窗口起點(diǎn)與終點(diǎn)的調(diào)度值，則該窗口的數(shù)學(xué)模型描述為W(R，α，β)，其中α=0°，β=180°，R=2m，R的取值和跟蹤時(shí)機(jī)器人與運(yùn)動(dòng)目標(biāo)理想的距離有關(guān)。

在激光掃描線(xiàn)與物體發(fā)生碰撞時(shí)，抵達(dá)物體到傳感器位置的距離后dj返回，在窗口邊緣之外的數(shù)據(jù)，如果di>dmax，則取di=dmax，即

(8)

式中，dmax屬于視窗內(nèi)運(yùn)動(dòng)目標(biāo)實(shí)測(cè)距離最大值的閾值，dmax=R，dj表示構(gòu)建視窗后第j個(gè)方向上的距離。

3.3 障礙空間聚類(lèi)

通過(guò)二維笛卡爾矩形柵格[9]展示雷達(dá)掃描到的跟蹤環(huán)境地圖，利用二維數(shù)據(jù)組記載該環(huán)境中相對(duì)的柵格是否存在障礙物。如果柵格只存在空閑與占據(jù)兩種情況，在已知每個(gè)柵格單元先驗(yàn)機(jī)率的基礎(chǔ)上，采樣時(shí)間點(diǎn)結(jié)合激光掃描到信息記載已知柵格的占據(jù)狀態(tài)。

假設(shè)任意一個(gè)柵格存在障礙物且被掃描到，則該柵格的數(shù)據(jù)值設(shè)置為1。假設(shè)k-近臨差值，Δρk為觀測(cè)點(diǎn)(ρi，j，λi，j)中第k個(gè)鄰近極角上點(diǎn)(ρi，j+k，λi，j+k)的距離，則

(9)

將測(cè)距信息根據(jù)下述原則劃分成障礙區(qū)域，通過(guò)式(9)對(duì)比機(jī)器人在采樣時(shí)間為i時(shí)兩個(gè)鄰近障礙物的距離Δρk，j與k分別從0和1開(kāi)始，當(dāng)k到達(dá)一定數(shù)值時(shí)，其附近區(qū)域中的最小差值△ρmin低于預(yù)測(cè)的閾值△ρset，此時(shí)j從0開(kāi)始，到k個(gè)鄰近角區(qū)域K中障礙全部當(dāng)作同一類(lèi)障礙。再?gòu)膉+k開(kāi)始，對(duì)障礙區(qū)域進(jìn)行逐次聚類(lèi)處理，在聚類(lèi)后，任意一個(gè)障礙子集存在的特征表示為：

1)質(zhì)心和質(zhì)量屬性

障礙子集中質(zhì)量被稱(chēng)為子集On的柵格數(shù)量Mn，它取決于On的范圍與柵格精準(zhǔn)度。子集On的質(zhì)心則取決于測(cè)量值和平均極角值[10]，即

(10)

(11)

Mn=k2-k1

(12)

根據(jù)機(jī)器人的位姿xrobot，yrobot，θrobot，將障礙集合On質(zhì)心轉(zhuǎn)化成全局坐標(biāo)形式的質(zhì)心坐標(biāo)，其表示為

xn=xrobot+ρncos(λn+θrobot)

(13)

yn=yrobot+ρnsin(λn+θrobot)

(14)

2)障礙子集的運(yùn)動(dòng)參數(shù)

利用數(shù)據(jù)關(guān)聯(lián)法則[11]，對(duì)函數(shù)獲得的障礙信息進(jìn)行評(píng)價(jià)，此時(shí)對(duì)障礙物運(yùn)動(dòng)速度評(píng)價(jià)表達(dá)式表示為

von=(On(i)-On(i-1))/Δt

(15)

式中，Δt表示采樣周期，獲取On的運(yùn)動(dòng)參數(shù)，對(duì)障礙物的運(yùn)動(dòng)狀態(tài)進(jìn)行評(píng)價(jià)，為后續(xù)跟蹤提供依據(jù)。

3.4 運(yùn)動(dòng)目標(biāo)檢測(cè)

結(jié)合上述思想，對(duì)運(yùn)動(dòng)目標(biāo)的起始與終止角度掃描的范圍進(jìn)行記載，并構(gòu)建檢測(cè)模型為(fk，sk，dmin)，f與s分別代表起始角與終止角，dmin為機(jī)器人和目標(biāo)存在的最短距離。則該目標(biāo)的位置坐標(biāo)(xk，yk)的計(jì)算表達(dá)式為

(16)

獲得機(jī)器人中點(diǎn)(xr，yr)與目標(biāo)中點(diǎn)(xk，yk)的距離表達(dá)式

(17)

在對(duì)運(yùn)動(dòng)目標(biāo)檢測(cè)過(guò)程中，會(huì)存在誤差，根據(jù)下述公式可對(duì)誤差進(jìn)行計(jì)算

σo=σd+σm

(18)

σm=(vr+vo)·(tsence+tprocess)

(19)

式中，σo為運(yùn)動(dòng)目標(biāo)測(cè)量誤差，σd表示激光掃描誤差，vr代表機(jī)器人運(yùn)動(dòng)速度。在已知上述誤差后對(duì)其進(jìn)行補(bǔ)償，從而提高跟蹤精準(zhǔn)度。

3.5 基于卡爾曼濾波的目標(biāo)跟蹤

卡爾曼濾波是對(duì)動(dòng)態(tài)目標(biāo)的狀態(tài)做線(xiàn)性最小誤差預(yù)測(cè)的方法。在本文中，激光掃描的運(yùn)動(dòng)目標(biāo)在圖像中，可以將其視為勻速運(yùn)動(dòng)，通過(guò)卡爾曼濾波器完成運(yùn)動(dòng)目標(biāo)的參數(shù)預(yù)測(cè)。

假設(shè)一個(gè)隨機(jī)動(dòng)態(tài)系統(tǒng)的數(shù)學(xué)模型表示為

S(k+1)=H(k)S+n(k)

(20)

X(k)=F(k)S(k)+η(k)

(21)

式(20)屬于狀態(tài)方程，S(k)表示系統(tǒng)狀態(tài)矢量，H(k)代表狀態(tài)矩陣，n(k)表示噪聲向量；式(21)為觀測(cè)方程，F(xiàn)(k)是觀測(cè)數(shù)據(jù)矩陣。并且n(k)與η(k)為均值是零的白噪聲，滿(mǎn)足高斯分布[12]

E[n(k)]=0，E[n(k)nT(i)]=Qk，i=k

(22)

E[η(k)]=0，E[η(k)ηT(i)]=Λk，i=k

(23)

定義機(jī)器人系統(tǒng)狀態(tài)向量表示為X[x，y，vx，vy]T，x與y分別代表在x軸與y軸中坐標(biāo)，vx和vy表示在兩個(gè)坐標(biāo)軸上的速度。在添加高斯干擾白噪聲后獲得運(yùn)動(dòng)目標(biāo)的狀態(tài)方程與觀測(cè)方程分別為

X(k)=A(k-1)X(k-1)+ω(k)

(24)

Z(k)=H(k)X(k)+v(k)

(25)

式中，A(k-1)是在T周期基礎(chǔ)上，從k+1時(shí)刻到k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，其描述為

(26)

(27)

式中，狀態(tài)噪聲ω(k)與觀測(cè)噪聲v(k)均為零，即

ω(k)≈N(0，Q)

(28)

v(k)≈N(0，R)

(29)

4 仿真分析

為證明所提方法的合理性，設(shè)置仿真，并通過(guò)與感知代理和移動(dòng)控制法則的分布式目標(biāo)跟蹤和無(wú)標(biāo)定視覺(jué)伺服機(jī)器人跟蹤控制的對(duì)比實(shí)驗(yàn)，驗(yàn)證方法的優(yōu)越性。

4.1 仿真環(huán)境和參數(shù)設(shè)置

本次仿真在MATLAB平臺(tái)上進(jìn)行，操作系統(tǒng)為WINDOWSXP，系統(tǒng)運(yùn)行內(nèi)存為8GB，CPU為3.6GHz。

仿真參數(shù)設(shè)置如下：機(jī)器人運(yùn)動(dòng)速度為5m/s，運(yùn)動(dòng)目標(biāo)速度是6m/s，掃描最遠(yuǎn)距離50m，測(cè)量間隔時(shí)間是0.3s，測(cè)量距離噪聲0.2m，測(cè)量角度噪聲π/180，跟蹤范圍是200m×200m。

4.2 結(jié)果分析

假設(shè)跟蹤的機(jī)器人表示為A，運(yùn)動(dòng)目標(biāo)為B，當(dāng)B從t0時(shí)間點(diǎn)沿y軸方向進(jìn)行直線(xiàn)運(yùn)動(dòng)，當(dāng)B與A的距離高于理想距離時(shí)，A開(kāi)始對(duì)B進(jìn)行跟蹤。采用不同方法對(duì)運(yùn)動(dòng)目標(biāo)跟蹤的軌跡結(jié)果如圖2 所示。

圖2 不同方法跟蹤軌跡對(duì)比圖

從圖中可以看出，本文算法的跟蹤軌跡與運(yùn)動(dòng)目標(biāo)軌跡更為接近，其它兩種方法都會(huì)出現(xiàn)在某一時(shí)間點(diǎn)偏離目標(biāo)軌跡的現(xiàn)象，這是由于所提方法在對(duì)運(yùn)動(dòng)目標(biāo)進(jìn)行跟蹤時(shí)，將激光掃描技術(shù)中引入卡爾曼濾波的方法，降低了跟蹤過(guò)程中其它噪聲等因素的干擾，提升了所提方法的跟蹤精度。

為進(jìn)一步驗(yàn)證所提方法的可行性，實(shí)驗(yàn)分析了所提方法、感知代理和移動(dòng)控制法則的分布式目標(biāo)跟蹤和無(wú)標(biāo)定視覺(jué)伺服機(jī)器人跟蹤控制方法對(duì)運(yùn)動(dòng)目標(biāo)跟蹤的效率，如圖3 所示：

圖3 不同方法跟蹤效率對(duì)比圖

分析圖3 可以看出，采用三種方法對(duì)運(yùn)動(dòng)目標(biāo)進(jìn)行跟蹤的效率不同。隨著跟蹤次數(shù)的改變，三種方法跟蹤效率始終呈現(xiàn)上升趨勢(shì)。其中，所提方法的跟蹤效率最高約為97%，而其它兩種方法的自動(dòng)跟蹤效率始終低于所提方法，驗(yàn)證了所提方法的可行性。

5 結(jié)論

針對(duì)傳統(tǒng)機(jī)器人對(duì)運(yùn)動(dòng)目標(biāo)進(jìn)行跟蹤時(shí)精度低的缺陷，本文在激光掃描基礎(chǔ)上對(duì)機(jī)器人跟蹤技術(shù)進(jìn)行研究。通過(guò)對(duì)機(jī)器人坐標(biāo)系與運(yùn)動(dòng)模型的建立，利用卡爾曼濾波器對(duì)運(yùn)動(dòng)目標(biāo)進(jìn)行跟蹤，實(shí)現(xiàn)了機(jī)器人運(yùn)動(dòng)目標(biāo)跟蹤。與傳統(tǒng)方法相比所提方法具有以下優(yōu)勢(shì)：

1)采用所提方法對(duì)目標(biāo)運(yùn)行軌跡的跟蹤效果與實(shí)際運(yùn)行效果較為接近，具有一定可信度；

2)采用所提方法對(duì)目標(biāo)運(yùn)行軌跡的跟蹤效率最高約為97%，具有一定可行性。