純方位角目標(biāo)跟蹤及移動(dòng)平臺(tái)可觀性控制方法

伍明，李廣宇，魏振華，汪洪橋

（1.西安高技術(shù)研究所 信息系統(tǒng)室，陜西 西安 710025; 2.陜西廣播電視大學(xué) 教學(xué)科，陜西 西安 710023）

智能移動(dòng)機(jī)器人是一類能夠通過傳感器感知環(huán)境和自身狀態(tài)，實(shí)現(xiàn)在有障礙物的環(huán)境中面向目標(biāo)的自主運(yùn)動(dòng)，從而完成一定作業(yè)功能的機(jī)器人系統(tǒng)[1]。從該定義可見，機(jī)器人對(duì)自身和環(huán)境對(duì)象（包括：靜態(tài)對(duì)象和動(dòng)態(tài)對(duì)象）的感知在任務(wù)執(zhí)行中起到關(guān)鍵性和基礎(chǔ)性作用。

單目視覺傳感器作為一種被動(dòng)式傳感器，憑借其體積小、耗能少、細(xì)節(jié)呈現(xiàn)度高等特點(diǎn)，正在機(jī)器人導(dǎo)航學(xué)中發(fā)揮著重要作用[2]。機(jī)器人和計(jì)算機(jī)視覺領(lǐng)域的相關(guān)學(xué)者已經(jīng)展開了基于單目視覺的機(jī)器人同時(shí)定位、地圖構(gòu)建與目標(biāo)跟蹤(simultaneous localization , mapping and object tracking,SLAMOT)問題研究[3]。機(jī)器人領(lǐng)域的SLAMOT以SLAM為核心[4-6]，目標(biāo)識(shí)別與跟蹤目的是提供更好的環(huán)境特征觀測(cè)值，通常假設(shè)目標(biāo)為靜止的結(jié)構(gòu)性物體且不考慮平臺(tái)可觀性機(jī)動(dòng)問題，Castler等[7]在 PTAM (parallel tracking and mapping)方法[8]基礎(chǔ)上提出了獨(dú)立于攝像機(jī)SFM (structure from motion)的目標(biāo)識(shí)別與跟蹤方法，該方法在圖像關(guān)鍵幀上通過特征點(diǎn)匹配識(shí)別環(huán)境目標(biāo)并利用扎集優(yōu)化方法(bundle adjustment, BA)實(shí)現(xiàn)對(duì)目標(biāo)空間位置的估計(jì)，但其假設(shè)目標(biāo)為靜止的畫像平面。De 等[9]將實(shí)際場(chǎng)景中運(yùn)動(dòng)物體融入地圖構(gòu)建以解決增強(qiáng)現(xiàn)實(shí)中虛擬物體投影參考缺失問題。Dai等[10]利用RGBD傳感器解決了動(dòng)態(tài)物體對(duì)SLAM的影響，但并未研究動(dòng)態(tài)物體跟蹤問題。Li等[11]研究了基于運(yùn)動(dòng)目標(biāo)跟蹤和靜止物體識(shí)別的語義學(xué)SLAM方法，運(yùn)功目標(biāo)跟蹤目的同樣是為了減少對(duì)SLAM的干擾。Civera等[12]將目標(biāo)識(shí)別與跟蹤納入到基于反轉(zhuǎn)深度的全關(guān)聯(lián)卡爾曼濾波框架[13]中，實(shí)現(xiàn)了環(huán)境物體的識(shí)別和空間位置的在線估計(jì)。同樣，該方法假設(shè)目標(biāo)為靜止的平面物體。Wangsiripitak等[14]設(shè)計(jì)了單目SLAM與目標(biāo)跟蹤同步估計(jì)算法，該算法處理的是空間立體靜止物體并未考慮目標(biāo)運(yùn)動(dòng)問題。Migliore等[15]設(shè)計(jì)了一種單目SLAM和動(dòng)態(tài)目標(biāo)跟蹤算法，該算法將傳統(tǒng)的SLAM濾波器和純方位角觀測(cè)目標(biāo)跟蹤器相結(jié)合，實(shí)現(xiàn)了對(duì)攝像機(jī)、環(huán)境特征和動(dòng)態(tài)目標(biāo)狀態(tài)的同時(shí)迭代估計(jì)，方法利用的平臺(tái)為手持?jǐn)z像機(jī)，因此并未考慮平臺(tái)可觀性機(jī)動(dòng)對(duì)目標(biāo)狀態(tài)估計(jì)的影響。

Bescos等[16]設(shè)計(jì)了一種SLAM和多目標(biāo)跟蹤相結(jié)合的方法，驗(yàn)證了動(dòng)態(tài)目標(biāo)跟蹤對(duì)SLAM的輔助作用。Liu等[17]提出了SLAM和目標(biāo)跟蹤強(qiáng)、弱耦合估計(jì)切換方法，有效結(jié)合了兩者的有點(diǎn)。Sualehm等[18]采用多傳感器融合方法實(shí)現(xiàn)動(dòng)態(tài)環(huán)境下SLAM任務(wù)，能夠完成對(duì)多個(gè)運(yùn)動(dòng)目標(biāo)的跟蹤。Liu等[19]研究了人造動(dòng)態(tài)環(huán)境下的SLAM問題，提出了動(dòng)態(tài)目標(biāo)跟蹤、視覺里程計(jì)和地圖構(gòu)建的多線程估計(jì)方法。計(jì)算機(jī)視覺領(lǐng)域的SLAMOT以基于單目視覺的目標(biāo)跟蹤為核心，又稱為：純方位角觀測(cè)目標(biāo)跟蹤(bearing only object tracking)或目標(biāo)運(yùn)動(dòng)分析(target motion analysis)，此類方法希望利用平臺(tái)機(jī)動(dòng)和目標(biāo)純方位角觀測(cè)，實(shí)現(xiàn)運(yùn)動(dòng)目標(biāo)的狀態(tài)估計(jì)，通常假設(shè)移動(dòng)平臺(tái)狀態(tài)已知。Oh等[20]提出了一種基于粒子群優(yōu)化的解決方法，但未考慮平臺(tái)的機(jī)動(dòng)優(yōu)化問題。Zhuk等[21]通過對(duì)比相鄰幀中跟蹤點(diǎn)實(shí)現(xiàn)運(yùn)動(dòng)物體檢測(cè)，但并未實(shí)現(xiàn)目標(biāo)狀態(tài)估計(jì)。Watanabe等[22]提出了基于單目視覺的移動(dòng)平臺(tái)導(dǎo)航方法，該方法針對(duì)單目視覺傳感器特點(diǎn)將導(dǎo)航控制和目標(biāo)狀態(tài)估計(jì)作為耦合問題處理，其假設(shè)移動(dòng)平臺(tái)狀態(tài)已知。Kim等[23]針對(duì)純方位角觀測(cè)目標(biāo)跟蹤問題提出了“雙重作用(dual effect)理論”認(rèn)為由于觀測(cè)深度信息缺失使得目標(biāo)狀態(tài)估計(jì)和平臺(tái)機(jī)動(dòng)運(yùn)動(dòng)之間存在關(guān)聯(lián)作用，目標(biāo)跟蹤準(zhǔn)確性極大依賴于平臺(tái)機(jī)動(dòng)方式，同樣，其假設(shè)平臺(tái)狀態(tài)已知。

綜上所述，學(xué)界已經(jīng)開展了基于單目視覺的SLAMOT問題的相關(guān)研究，但通常將SLAM和OT相互割裂，并未考慮目標(biāo)的移動(dòng)性、機(jī)器人狀態(tài)未知性以及平臺(tái)的可觀性優(yōu)化控制問題。文章正是針對(duì)以上問題展開研究，提出了基于單目視覺的機(jī)器人同時(shí)定位、地圖構(gòu)建與目標(biāo)跟蹤及平臺(tái)控制方法，該方法利用單目視覺SLAM實(shí)現(xiàn)對(duì)機(jī)器人狀態(tài)估計(jì)，并結(jié)合目標(biāo)觀測(cè)模型進(jìn)行基于擴(kuò)展式卡爾曼濾波的目標(biāo)狀態(tài)估計(jì)。為了克服觀測(cè)模型缺陷，提出了基于目標(biāo)協(xié)方差陣更新最大化的機(jī)器人優(yōu)化控制方法，最終實(shí)現(xiàn)了未知環(huán)境下基于單目視覺的移動(dòng)機(jī)器人目標(biāo)跟蹤。

1 問題描述和模型構(gòu)建

1.1 問題描述

假設(shè)k時(shí)刻機(jī)器人狀態(tài)為，環(huán)境特征狀態(tài)為,i=1,2,···,m，目標(biāo)狀態(tài)為，機(jī)器人利用單目視覺傳感器對(duì)目標(biāo)和環(huán)境特征進(jìn)行觀測(cè)且觀測(cè)值分別為和，方法處理目的是在機(jī)器人端實(shí)現(xiàn)對(duì)，及,i=1,2,···,m的在線估計(jì)。由于單目攝像機(jī)得到的觀測(cè)量缺少距離信息，存在可觀性問題。為了估計(jì)目標(biāo)與機(jī)器人的相對(duì)距離，機(jī)器人需要根據(jù)目標(biāo)不確定分布實(shí)時(shí)計(jì)算控制量urk，以保證在該控制量作用下得到的前后時(shí)序目標(biāo)觀測(cè)值和之間存在足夠視差，從而為距離估計(jì)提供足夠的信息。

相關(guān)對(duì)象狀態(tài)演變符合馬爾可夫過程，其對(duì)應(yīng)的貝葉斯網(wǎng)絡(luò)模型如圖1所示。其中=為機(jī)器人狀態(tài)和環(huán)境特征狀態(tài)擴(kuò)展構(gòu)成的SLAM系統(tǒng)狀態(tài)向量。系統(tǒng)的兩條主線為估計(jì)流程和估計(jì)流程，的估計(jì)采用基于 反轉(zhuǎn)深度的單目視覺SLAM方法[12]。估計(jì)過程基于擴(kuò)展式卡爾曼濾波，首先，根據(jù)目標(biāo)運(yùn)動(dòng)模型預(yù)測(cè)目標(biāo)狀態(tài)和協(xié)方差陣，第二，利用SLAM環(huán)節(jié)得到的機(jī)器人狀態(tài)和協(xié)方差，結(jié)合目標(biāo)觀測(cè)模型得到目標(biāo)預(yù)測(cè)觀測(cè)值和觀測(cè)殘差陣，最后，對(duì)預(yù)測(cè)狀態(tài)和協(xié)方差陣進(jìn)行更新，得到和。

圖1 貝葉斯網(wǎng)絡(luò)模型Fig.1 Bayesian network model

1.2 數(shù)學(xué)模型構(gòu)建

1）目標(biāo)運(yùn)動(dòng)模型

目標(biāo)k時(shí)刻狀態(tài)由各坐標(biāo)軸分量及其速度構(gòu)成，即，假設(shè)目標(biāo)運(yùn)動(dòng)模式為定速度模型(constant velocity model, CVM)[24]，則CVM模型的離散形式可表示成為

2）機(jī)器人運(yùn)動(dòng)模型

機(jī)器人運(yùn)動(dòng)模型符合運(yùn)動(dòng)方向和線速度解耦的多旋翼無人機(jī)模型，設(shè)k時(shí)刻狀態(tài)為其中為空間位置向量，為朝向4元組向量。

假設(shè)k時(shí)刻機(jī)器人的控制量為其中為位移控制分量，為角度控制分量，則機(jī)器人的狀態(tài)更新可分解位置和朝向更新，即：

其中，R(qr)是世界坐標(biāo)系到機(jī)器人坐標(biāo)系的旋轉(zhuǎn)矩陣，其值可根據(jù)qr計(jì)算， Ω (Δφr)為朝向3元組到4元組的反對(duì)稱矩陣，表示為

3）目標(biāo)觀測(cè)模型

式中：s=[susvu0v0]′為攝像機(jī)內(nèi)參向量，d=[d1d2]′為攝像機(jī)畸變系數(shù)向量，s和d由文獻(xiàn)[16]介紹的相機(jī)標(biāo)定方法獲得。r是均值為0，標(biāo)準(zhǔn)差為 σr的觀測(cè)加性白噪聲。

2 系統(tǒng)總體框架

系統(tǒng)總體框架如圖2所示。

圖2 系統(tǒng)總體框架Fig.2 Overall framework of the system

機(jī)器人外感傳感器是前向單目攝像機(jī)，每輪觀測(cè)對(duì)拍攝的圖片進(jìn)行環(huán)境特征提取以及目標(biāo)識(shí)別進(jìn)而獲得目標(biāo)觀測(cè)值與環(huán)境特征觀測(cè)值。系統(tǒng)包含3個(gè)核心處理模塊：1)單目SLAM處理模塊；2)單目視覺OT模塊；3)機(jī)器人控制量生成模塊。其中，單目SLAM處理模塊采用基于反轉(zhuǎn)深度參數(shù)表示法的全概率卡爾曼濾波方法實(shí)現(xiàn)機(jī)器人狀態(tài)和協(xié)方差陣的估計(jì)；單目OT模塊利用和結(jié)合此時(shí)目標(biāo)觀測(cè)值進(jìn)行基于擴(kuò)展式卡爾曼濾波的目標(biāo)跟蹤得到目標(biāo)狀態(tài)和協(xié)方差陣；由于單目視覺傳感器缺少深度觀測(cè)值使目標(biāo)狀態(tài)估計(jì)出現(xiàn)可觀性問題，即，目標(biāo)狀態(tài)無法利用機(jī)器人狀態(tài)和觀測(cè)值直接推導(dǎo)出，因此在機(jī)器人控制量生成環(huán)節(jié)，需要利用優(yōu)化控制方法生成機(jī)器人控制量，以保證機(jī)器人對(duì)目標(biāo)的跟隨和目標(biāo)狀態(tài)的有效估計(jì)。

3 機(jī)器人單目視覺SLAM

采用文獻(xiàn)[9]提出的反轉(zhuǎn)深度單目視覺SLAM方法，基本處理流程圖3所示。

圖3 VSLAM處理流程Fig.3 Processing flow of VSLAM

4 單目視覺移動(dòng)機(jī)器人目標(biāo)跟蹤方法

目標(biāo)跟蹤采用擴(kuò)展式卡爾曼濾波框架，假設(shè)k時(shí)刻機(jī)器人的狀態(tài)和協(xié)方差由第3節(jié)介紹方法獲得。目標(biāo)跟蹤的處理過程如下。

1）目標(biāo)狀態(tài)、協(xié)方差預(yù)測(cè)

目標(biāo)狀態(tài)預(yù)測(cè)公式如下：

2）目標(biāo)觀測(cè)值及觀測(cè)殘差陣預(yù)測(cè)

其中，

3）目標(biāo)狀態(tài)、協(xié)方差更新

其中，K為卡爾曼增益陣，為式（6）對(duì)的雅可比陣。

5 基于目標(biāo)協(xié)方差陣更新最大化的機(jī)器人優(yōu)化控制方法

機(jī)器人控制要達(dá)到兩個(gè)目的：首先，要確保機(jī)器人對(duì)目標(biāo)的追蹤，使目標(biāo)始終在機(jī)器人視野內(nèi)并保持一定距離；第二，保證機(jī)器人能夠在純方位角觀測(cè)條件下對(duì)目標(biāo)狀態(tài)的準(zhǔn)確估計(jì)。優(yōu)化控制量生成過程如圖4示。

圖4 優(yōu)化控制量生成過程Fig.4 Generation process of optimal control value

機(jī)器人控制量包括角度控制3元組 Δ φk和位置控制3元組 Δxk，首先，產(chǎn)生角度控制量 Δ φk，該控制分量要保證機(jī)器人始終朝向目標(biāo)。之后，產(chǎn)生位移控 制 量 Δxk， Δxk由跟隨控制分量 Δxfk和可觀性控制分量 Δxak組成。

5.1 角度控制 Δ φk 計(jì)算

假設(shè)k時(shí)刻，目標(biāo)觀測(cè)值為=[utvt]′，根據(jù)攝像機(jī)小孔成像模型可知：

式中：xC,r→t為攝像機(jī)坐標(biāo)系中，成像焦點(diǎn)指向目標(biāo)的向量；u0、v0、su、sv為 攝像機(jī)內(nèi)參；RC→R為機(jī)器人坐標(biāo)系中攝像機(jī)相對(duì)于機(jī)器人的旋轉(zhuǎn)矩陣。

假設(shè)k時(shí)刻，機(jī)器人角度控制量為 Δ φk=[? θψ]′，分量表示機(jī)器人坐標(biāo)系中繞X軸（旋轉(zhuǎn)roll），Y軸（俯仰pitch）和Z軸（偏航y(tǒng)aw）的角度增量。Δφk的計(jì)算公式為

5.2 位移控制量 Δ xk 計(jì)算

假設(shè)k時(shí)刻，機(jī)器人位置控制量為 Δxk，其計(jì)算公式為

其中，

V為機(jī)器人速度常量，為世界坐標(biāo)系到機(jī)器人坐標(biāo)系的旋轉(zhuǎn)矩陣，為跟隨控制分量，為產(chǎn)生目標(biāo)觀測(cè)視差的可觀性控制分量。

留給ofo的時(shí)間已經(jīng)不多。11月14日，久未露面的戴威在已經(jīng)很久沒有舉行的ofo公司大會(huì)上表示：除了破產(chǎn)，其他都有可能。

其中 λ ∈(01]為轉(zhuǎn)換速度參量，其值越大轉(zhuǎn)換速度越快，為k時(shí)刻機(jī)器人與目標(biāo)之間的距離，Deq為機(jī)器人和目標(biāo)間的平衡距離。

圖5為V=5m/s，Deq=15m，λ分別取0.3、0.5、1，為0～30連續(xù)變換時(shí)形成的曲線。

圖5 隨 變化曲線Fig.5 Curve graph of with

圖6 純追隨模式下不同時(shí)刻，目標(biāo)不確定橢圓變化圖Fig.6 Target uncertain elliptic change diagram at different times in pure follow-up mode

Object:

其中，Vt為式（9）中對(duì)應(yīng)的位置不確定橢圓體積，B＞0為加權(quán)常量陣。

因?yàn)椋?/p>

重寫式（16）為

Object:

則最終可得目標(biāo)函數(shù)為

Object:

根據(jù)式（10）有：

將式（21）代入式（20）可得目標(biāo)函數(shù)最終形式為

Object:

下面給出式（23）的具體求解方法，在不產(chǎn)生歧義的情況下，將寫成，寫成。

假設(shè)k時(shí)刻機(jī)器人位置狀態(tài)為，目標(biāo)位置狀態(tài)為，機(jī)器人可觀性控制量為，則世界坐標(biāo)系中從機(jī)器人到目標(biāo)的向量為

根據(jù)單目攝像機(jī)小孔成像模型有：

由偏導(dǎo)鏈?zhǔn)皆瓌t可知：

根據(jù)式（24）～（26）分別計(jì)算式（27）中各項(xiàng)整理可得：

其中，

將式（28）代入式（23）整理可得：

其中，

將式（35）、（36）代入式（34）即可得到最優(yōu)控制分量。

實(shí)現(xiàn)未知環(huán)境下基于單目視覺的機(jī)器人目標(biāo)跟蹤可觀性控制需要完成以下3方面計(jì)算：首先，完成機(jī)器人同時(shí)定位于地圖構(gòu)建，第二，完成目標(biāo)狀態(tài)跟蹤，第三，根據(jù)機(jī)器人和目標(biāo)狀態(tài)完成機(jī)器人控制量計(jì)算。計(jì)算復(fù)雜度主要集中在機(jī)器人SLAM過程中，由于機(jī)器人SLAM、目標(biāo)跟蹤和控制量生成在計(jì)算上是相互解耦的，因此可以選用運(yùn)算效率較高的SLAM方法（FAST-SLAM等）。

6 實(shí)驗(yàn)設(shè)計(jì)與驗(yàn)證分析

6.1 實(shí)驗(yàn)設(shè)計(jì)

1）仿真實(shí)驗(yàn)設(shè)計(jì)

下面通過仿真實(shí)驗(yàn)驗(yàn)證方法的有效性、準(zhǔn)確性并分析影響目標(biāo)跟蹤準(zhǔn)確性的因素。實(shí)驗(yàn)仿真環(huán)境為MATLAB2017，場(chǎng)景如圖7所示。

圖7 仿真實(shí)驗(yàn)場(chǎng)景Fig.7 Experiment scene of simulation

實(shí)驗(yàn)場(chǎng)景為三維立體空間，其范圍為：X: -25 m～25 m，Y: -25 m～25 m，Z: -10 m～10 m，在空間四周均勻地分布著環(huán)境特征點(diǎn)。相關(guān)實(shí)驗(yàn)參數(shù)配置如表1所示。

表1 實(shí)驗(yàn)參數(shù)配置表Table 1 Parameter configuration

2）實(shí)體機(jī)器人實(shí)驗(yàn)設(shè)計(jì)

實(shí)體機(jī)器人實(shí)驗(yàn)采用Turtlebot2型差動(dòng)驅(qū)動(dòng)平臺(tái)，如圖8所示。

圖8 Turtlebot2實(shí)體機(jī)器人平臺(tái)Fig.8 Turtlebot2 physical robot platform

該平臺(tái)配備單目攝像頭，WiFi無線通信系統(tǒng)以及UWB（ultra wide band, UWB）室內(nèi)定位系統(tǒng)。單目攝像頭完成RGB圖像獲取，無線通信系統(tǒng)完成機(jī)器人端和服務(wù)器端的數(shù)據(jù)傳輸，UWB定位系統(tǒng)提供機(jī)器人和目標(biāo)的定位真值。實(shí)驗(yàn)場(chǎng)景如圖9所示。

圖9 實(shí)驗(yàn)場(chǎng)景Fig.9 Experimental scene

實(shí)驗(yàn)環(huán)境為室內(nèi)環(huán)境，載有色塊的機(jī)器人作為目標(biāo)進(jìn)行勻速直線運(yùn)動(dòng)，后方機(jī)器人作為跟蹤機(jī)器人，環(huán)境中4個(gè)UWB基站作為定位標(biāo)準(zhǔn)點(diǎn)且位置已知。

實(shí)驗(yàn)基于 ROS（robot operating system, ROS）框架，節(jié)點(diǎn)及消息關(guān)系如圖10所示。

圖10 ROS框架節(jié)點(diǎn)及消息關(guān)系實(shí)驗(yàn)場(chǎng)景Fig.10 ROS framework node and message diagram

考慮到圖像數(shù)據(jù)量巨大不利于實(shí)時(shí)傳輸，以及Turtlebot上位機(jī)運(yùn)算能力有限不利于計(jì)算等因素，系統(tǒng)將感知與執(zhí)行，對(duì)象狀態(tài)估計(jì)與控制量生成分別在機(jī)器人端和運(yùn)算服務(wù)器端完成。Turtlebot機(jī)器人端包含感知節(jié)點(diǎn)和執(zhí)行節(jié)點(diǎn)分別完成環(huán)境特征檢測(cè)、目標(biāo)檢測(cè)和平臺(tái)運(yùn)動(dòng)控制任務(wù)，其中環(huán)境特征為Sift特征點(diǎn)，目標(biāo)檢測(cè)利用Camshift方法實(shí)現(xiàn)。運(yùn)算服務(wù)器端包含SLAM運(yùn)算節(jié)點(diǎn)、目標(biāo)跟蹤節(jié)點(diǎn)和控制量計(jì)算節(jié)點(diǎn)分別利用第3、4、5節(jié)介紹方法實(shí)現(xiàn)機(jī)器人狀態(tài)估計(jì)、目標(biāo)狀態(tài)估計(jì)和平臺(tái)控制量生成任務(wù)。另外，由于該機(jī)器人運(yùn)動(dòng)學(xué)受非完整性約束限制，采用文獻(xiàn)[26]所介紹的控制方法實(shí)現(xiàn)完整性約束到非完整性約束的轉(zhuǎn)換控制。

6.2 仿真實(shí)驗(yàn)驗(yàn)證及分析

6.2.1 有效性實(shí)驗(yàn)分析

為了驗(yàn)證方法有效性，假設(shè)目標(biāo)運(yùn)動(dòng)軌跡為直線，初始位置坐標(biāo)為 (1 5,15,1)，速度恒定為2 m/s。機(jī)器人初始位置坐標(biāo)為 (1 5,-15,1)。機(jī)器人與目標(biāo)的平衡距離設(shè)為Deq=10m，機(jī)器人最大機(jī)動(dòng)速度設(shè)為vmax=2.5m/s，仿真共進(jìn)行了300次迭代，時(shí)間步進(jìn)為0.05 s。

實(shí)驗(yàn)總體跟蹤結(jié)果俯視圖如圖11所示。圖中目標(biāo)和機(jī)器人實(shí)際軌跡為實(shí)線，目標(biāo)和機(jī)器人估計(jì)軌跡為虛線，十字點(diǎn)為環(huán)境特征實(shí)際位置，橢圓為環(huán)境特征狀態(tài)估計(jì)分布橢圓。從圖可見，實(shí)驗(yàn)開始時(shí)機(jī)器人距離目標(biāo)較遠(yuǎn)，控制以追隨為主，當(dāng)機(jī)器人距離目標(biāo)較近時(shí)可觀性控制分量開始發(fā)揮作用，機(jī)器人做機(jī)動(dòng)運(yùn)動(dòng)。從目標(biāo)估計(jì)軌跡可見，剛開始由于機(jī)器人并未產(chǎn)生可觀性機(jī)動(dòng)，因此目標(biāo)估計(jì)軌跡逐漸偏離實(shí)際軌跡，當(dāng)機(jī)器人開始機(jī)動(dòng)時(shí)，目標(biāo)估計(jì)軌跡快速收斂到實(shí)際軌跡附近。

圖11 實(shí)驗(yàn)總體跟蹤結(jié)果Fig.11 Overall tracking results

圖12為三維空間中機(jī)器人實(shí)際和估計(jì)軌跡的局部放大圖。由圖可見，機(jī)器人的機(jī)動(dòng)曲線為螺旋式曲線。機(jī)器人以此種軌跡對(duì)目標(biāo)進(jìn)行跟隨有利于產(chǎn)生足夠的單目觀測(cè)視差，從而快速完成相對(duì)目標(biāo)距離的估計(jì)。

圖12 三維空間機(jī)器人軌跡局部放大圖Fig.12 A local large robot trajectory in three dimensional space

圖13為實(shí)驗(yàn)中不同時(shí)刻機(jī)器人控制量成分值變化曲線。圖中實(shí)線為不同時(shí)刻機(jī)器人速度控制值，細(xì)虛線為其中追隨控制分量值，粗虛線為其中可觀性控制分量值，由圖可見，在跟蹤初期0～85 s期間，機(jī)器人控制量的主要成分為追隨控制分量。隨著機(jī)器人靠近目標(biāo)可觀性控制分量開始發(fā)揮作用，在時(shí)段85～300 s期間，可觀性控制成分大于追隨控制成分，因此，在此期間機(jī)器人開始螺旋式機(jī)動(dòng)運(yùn)動(dòng)，從而克服單目視覺深度觀測(cè)值缺失的問題，快速實(shí)現(xiàn)目標(biāo)位置狀態(tài)估計(jì)。

圖13 不同時(shí)刻機(jī)器人控制分量變化曲線Fig.13 The curve of control component of robot at different time

圖14為不同時(shí)刻機(jī)器人和目標(biāo)各分量及位置估計(jì)誤差曲線。

圖14 目標(biāo)和機(jī)器人各分量及位置估計(jì)誤差曲線Fig.14 Estimation error curve of each components and position

從圖14(a)可見，在0～85 s期間，由于機(jī)器人并未進(jìn)行可觀性機(jī)動(dòng)，因此，目標(biāo)各分量及位置誤差持續(xù)增加。在85～300 s期間，機(jī)器人開始機(jī)動(dòng)運(yùn)動(dòng)，目標(biāo)各分量及位置誤差快速減小并始終保持在較低水平。從圖14(b)可見，機(jī)器人各分量及位置估計(jì)始終較為準(zhǔn)確，從圖11可見，環(huán)境特征分布均勻且機(jī)器人在運(yùn)動(dòng)過程中有足夠多的、準(zhǔn)確性高的特征保持在其視野中，這些保證了較高的機(jī)器人定位精度。

圖15 不同時(shí)刻目標(biāo)觀測(cè)值不確定橢圓面積變化曲線Fig.15 Target observation uncertain elliptical area change curve

由圖可見，在0～85 s期間，觀測(cè)不確定橢圓面積變化很平穩(wěn)，說明機(jī)器人對(duì)目標(biāo)的前后時(shí)序觀測(cè)視差變化較小，用來進(jìn)行深度估計(jì)的信息量較少，因此，目標(biāo)狀態(tài)估計(jì)誤差持續(xù)增大。在85 s，出現(xiàn)了一個(gè)較大跳變，此后機(jī)器人開始可觀性機(jī)動(dòng)運(yùn)動(dòng)，在此期間，觀測(cè)不確定橢圓面積變化較為劇烈，這說明機(jī)器人在前后觀測(cè)中存在較大視差，能夠?yàn)槟繕?biāo)深度估計(jì)提供足夠多的信息量，從而保證了目標(biāo)跟蹤的準(zhǔn)確性。

6.2.2 準(zhǔn)確性實(shí)驗(yàn)及分析

為了驗(yàn)證方法的準(zhǔn)確性，假設(shè)目標(biāo)起點(diǎn)與終點(diǎn)均為 (0 ,15,1)，目標(biāo)實(shí)際運(yùn)行軌跡為圓形，線速度為 2 m/s 。機(jī)器人起點(diǎn)為 (1 0,15,1)，朝向?yàn)?(0 ,0,π)，機(jī)器人與目標(biāo)的平衡距離設(shè)為Deq=10m，機(jī)器人最大機(jī)動(dòng)速度設(shè)為vmax=2.5m/s，仿真共進(jìn)行了500次迭代，時(shí)間步進(jìn)為0.05 s。實(shí)驗(yàn)總圖跟蹤結(jié)果如圖16所示。

圖16 實(shí)驗(yàn)總體結(jié)果Fig.16 Overall tracking results

從結(jié)果可知，機(jī)器人和目標(biāo)的估計(jì)軌跡和實(shí)際軌跡保持較高的一致性。

圖17為機(jī)器人和目標(biāo)軌跡及誤差曲線。

圖17 機(jī)器人和目標(biāo)軌跡及誤差曲線Fig.17 Robot , target trajectory and error curve

上方兩個(gè)子圖為機(jī)器人和目標(biāo)的實(shí)際與估計(jì)軌跡對(duì)比圖，下方兩個(gè)子圖為不同時(shí)刻機(jī)器人和目標(biāo)位置估計(jì)誤差曲線圖，從軌跡圖可見，機(jī)器人和目標(biāo)估計(jì)軌跡始終貼合實(shí)際軌跡。從誤差曲線圖可見，機(jī)器人和目標(biāo)估計(jì)誤差均保持在容差范圍內(nèi)。另外，在整個(gè)估計(jì)過程中機(jī)器人平均估計(jì)誤差為0.202 m，目標(biāo)平均估計(jì)誤差為1.227 m，目標(biāo)和機(jī)器人估計(jì)準(zhǔn)確性相差一個(gè)數(shù)量級(jí)。

6.2.3 影響因素實(shí)驗(yàn)及分析

下面分別分析機(jī)器人定位精度、機(jī)器人機(jī)動(dòng)程度兩種因素對(duì)目標(biāo)跟蹤精度的影響。

假設(shè)機(jī)器人最大速度為vmax=3.5m/s ，平衡距離Deq=10m條件下，機(jī)器人位置估計(jì)在10種噪聲干擾下得到的目標(biāo)狀態(tài)估計(jì)誤差均值和方差曲線如圖18所示。

圖18 機(jī)器人定位誤差對(duì)目標(biāo)定位精度影響曲線Fig.18 Influence between robot and target localization accuracy

由圖可見，隨著人為引入機(jī)器人定位噪聲的增加，當(dāng)加性機(jī)器人定位高斯白噪聲的標(biāo)準(zhǔn)差為1時(shí)，目標(biāo)定位誤差均值為16.36，誤差標(biāo)準(zhǔn)差為5.83。由此可見，目標(biāo)定位精度與機(jī)器人定位精度呈現(xiàn)正相關(guān)性。

圖19為假設(shè)平衡距離Deq=10m，機(jī)器人最大速度vmax取10個(gè)不同值時(shí)對(duì)應(yīng)的目標(biāo)狀態(tài)估計(jì)誤差均值和方差曲線。

圖19 機(jī)動(dòng)能力對(duì)目標(biāo)定位精度影響曲線Fig.19 Influence between robot maneuver ability and target localization accuracy

由圖可見，隨著機(jī)器人最大運(yùn)動(dòng)速度vmax的增大，目標(biāo)狀態(tài)估計(jì)準(zhǔn)確性逐步提高并最終趨近于恒定值。說明目標(biāo)定位精度與機(jī)器人機(jī)動(dòng)能力成正相關(guān)性。

6.3 實(shí)體機(jī)器人實(shí)驗(yàn)驗(yàn)證及分析

實(shí)體機(jī)器人實(shí)驗(yàn)中，目標(biāo)初始位置坐標(biāo)為( 0 ,-2)，速度恒定為0.07 m/s。機(jī)器人初始位置坐標(biāo)為(2,-4)。機(jī)器人與目標(biāo)的平衡距離設(shè)為Deq=1m，機(jī)器人最大機(jī)動(dòng)速度為vmax=0.2m/s，實(shí)驗(yàn)共進(jìn)行了60 s。需要說明的是，由于所選用的機(jī)器人平臺(tái)較為簡(jiǎn)易，其單目攝像頭視野狹窄，同時(shí)，易產(chǎn)生圖像晃動(dòng)問題。因此，在實(shí)驗(yàn)中有意將目標(biāo)和機(jī)器人運(yùn)動(dòng)速度調(diào)慢，意在弱化條件，重點(diǎn)檢驗(yàn)方法對(duì)目標(biāo)狀態(tài)，機(jī)器人狀態(tài)估計(jì)的有效性和準(zhǔn)確性。

實(shí)體機(jī)器人跟蹤結(jié)果如圖20所示。

圖20 實(shí)體機(jī)器人跟蹤結(jié)果Fig.20 Physical robot tracking results

由圖20(b)可見，由于機(jī)器人和目標(biāo)在相同平面運(yùn)動(dòng)，為了產(chǎn)生觀測(cè)視差，機(jī)器人形成曲線運(yùn)動(dòng)軌跡，同時(shí)估計(jì)軌跡和實(shí)際軌跡一致。目標(biāo)估計(jì)軌跡和實(shí)際軌跡開始一致性較差，之后逐步趨于一致，其原因在于初始階段機(jī)器人以追隨為主并未產(chǎn)生足夠視差，當(dāng)機(jī)器人接近目標(biāo)后開始曲線運(yùn)動(dòng)，足夠的視差使目標(biāo)跟蹤精度得以提高。

7 結(jié)論

針對(duì)未知環(huán)境下基于單目視覺的移動(dòng)機(jī)器人目標(biāo)跟蹤問題展開研究，提出了一種純方位角觀測(cè)條件下的機(jī)器人同時(shí)定位、地圖構(gòu)建與目標(biāo)跟蹤及控制方法。該方法在機(jī)器人單目視覺SLAM基礎(chǔ)上，設(shè)計(jì)了擴(kuò)展式卡爾曼濾波框架的目標(biāo)跟蹤算法，并將目標(biāo)狀態(tài)估計(jì)和機(jī)器人運(yùn)動(dòng)控制作為耦合問題進(jìn)行處理，提出了基于目標(biāo)協(xié)方差陣更新最大化的機(jī)器人優(yōu)化控制方法。仿真和實(shí)體實(shí)驗(yàn)驗(yàn)證了方法的有效性和準(zhǔn)確性，通過對(duì)實(shí)驗(yàn)數(shù)據(jù)的分析可知：目標(biāo)跟蹤準(zhǔn)確性與機(jī)器人定位精度以及機(jī)器人機(jī)動(dòng)能力大小成正相關(guān)性。接下來將針對(duì)障礙物條件下的估計(jì)和控制方法展開進(jìn)一步研究。