基于計(jì)算機(jī)視覺(jué)的輪椅跟隨系統(tǒng)設(shè)計(jì)

李 艷，唐達(dá)明，2，周瑩亮，戴慶瑜

1.陜西科技大學(xué) 電氣與控制工程學(xué)院，西安710021

2.陜西農(nóng)產(chǎn)品加工技術(shù)研究院，西安710021

隨著人口老年化逐漸加劇，老年人的看護(hù)需求快速增加。在醫(yī)院、養(yǎng)老院和一些醫(yī)療條件好的看護(hù)機(jī)構(gòu)，老年人和殘疾人的數(shù)量多，生活自理能力較差，需要大量的看護(hù)人員來(lái)進(jìn)行看護(hù)，由于看護(hù)任務(wù)大，使得看護(hù)很難做到面面俱到，因此如何保障老年人和處于康復(fù)階段的人群的生活質(zhì)量迫在眉睫[1]。對(duì)于他們來(lái)說(shuō)，久坐和長(zhǎng)時(shí)間的躺臥均不利于身體健康，他們不能長(zhǎng)時(shí)間行走，為了更快地恢復(fù)身體又需要經(jīng)常鍛煉，他們需要一種不僅能夠?qū)崿F(xiàn)日常代步功能，而且能夠在鍛煉身體的時(shí)候?qū)λ麄兊陌踩M(jìn)行監(jiān)護(hù)，當(dāng)身體鍛煉結(jié)束后能得到及時(shí)休息的智能輔助工具，因此設(shè)計(jì)一款具有目標(biāo)自動(dòng)跟隨功能的輪椅顯得尤為重要[2]。

目前的跟隨輪椅主要使用兩種傳感器，一種是距離傳感器，另一種是視覺(jué)傳感器。與傳統(tǒng)的距離傳感器如雷達(dá)[3]、激光[4]、紅外線[5]等相比，計(jì)算機(jī)視覺(jué)傳感器具有以下優(yōu)點(diǎn)：（1）不會(huì)受系統(tǒng)中其他傳感器的干擾而影響檢測(cè)精度；（2）具有與被觀測(cè)的對(duì)象無(wú)接觸、直觀快速和測(cè)量范圍廣的優(yōu)點(diǎn)；（3）具有對(duì)特定目標(biāo)識(shí)別的優(yōu)點(diǎn)，能夠同時(shí)采集視野中的多個(gè)信息用于各種功能的使用，例如檢測(cè)摔倒、回傳圖片及環(huán)境信息等。因此目前對(duì)基于視覺(jué)的輪椅自動(dòng)跟隨控制方式開(kāi)展了大量研究，并取得了豐碩的研究成果[6]。Μotokucho 等人[7]設(shè)計(jì)并實(shí)現(xiàn)了一種基于視覺(jué)的動(dòng)力輔助輪椅跟隨控制系統(tǒng)，其采用光流矢量提取前方目標(biāo)人員腿部的特性，進(jìn)而確定目標(biāo)人員相對(duì)于輪椅的位置，完成跟隨，但是當(dāng)周?chē)嬖谳^多的干擾人員時(shí)，會(huì)造成目標(biāo)人員的腿部信息被嚴(yán)重遮擋，只能應(yīng)用于簡(jiǎn)單的環(huán)境中。包加桐等人[8]設(shè)計(jì)了一種電動(dòng)輪椅跟隨目標(biāo)人物移動(dòng)的控制系統(tǒng)，利用TLD（Tracking-Learning-Detection）跟蹤算法跟蹤視頻圖像中的目標(biāo)人員，同時(shí)結(jié)合深度圖像，得到目標(biāo)人員位置信息，但是由于只使用單一的顏色特征作為目標(biāo)的跟蹤特征，在室外環(huán)境中目標(biāo)跟隨的過(guò)程容易受到光照和其他相似目標(biāo)的影響，造成目標(biāo)的丟失問(wèn)題[9]。

目前，國(guó)內(nèi)外學(xué)者對(duì)基于視覺(jué)的輪椅自動(dòng)跟隨控制方式開(kāi)展了大量研究，但這些控制方式大多需要輪椅用戶(hù)在大型室內(nèi)環(huán)境或室外簡(jiǎn)單環(huán)境中他人的引導(dǎo)或陪伴。由于輪椅目標(biāo)跟隨控制方式的復(fù)雜性，仍存在一些問(wèn)題需要繼續(xù)研究和探索：（1）大多數(shù)的輪椅跟隨系統(tǒng)需要手動(dòng)初始化待跟蹤的目標(biāo)，這樣降低了系統(tǒng)的便捷性，而且在一定程度上增加了人為誤差；（2）輪椅目標(biāo)跟隨系統(tǒng)采用單一的特征作為目標(biāo)跟蹤特征，當(dāng)出現(xiàn)外觀相似的目標(biāo)時(shí)，容易造成目標(biāo)跟蹤失敗，而且在復(fù)雜背景下，容易對(duì)目標(biāo)跟蹤的準(zhǔn)確性造成一定的干擾。

針對(duì)以上問(wèn)題，本文設(shè)計(jì)的輪椅跟隨系統(tǒng)圍繞如何在光照變化、目標(biāo)遮擋以及復(fù)雜背景下提高傳統(tǒng)目標(biāo)跟蹤算法的跟蹤精度和準(zhǔn)確率，進(jìn)而應(yīng)用于嵌入式設(shè)備中，實(shí)現(xiàn)輪椅對(duì)目標(biāo)穩(wěn)定、安全地跟隨。

1 輪椅的目標(biāo)檢測(cè)與跟蹤算法

由于傳統(tǒng)的目標(biāo)跟蹤算法無(wú)法自動(dòng)完成目標(biāo)的初始化，一般需要手動(dòng)選取視頻序列第一幀的目標(biāo)圖像作為搜索框，因此傳統(tǒng)的目標(biāo)跟蹤算法屬于半自動(dòng)的跟蹤算法，而且在一定程度上手動(dòng)初始化目標(biāo)的搜索窗口也會(huì)造成人為誤差。基于深度學(xué)習(xí)的目標(biāo)跟蹤算法需要前期進(jìn)行大量的訓(xùn)練，在目標(biāo)發(fā)生變化時(shí)需要重新進(jìn)行訓(xùn)練，且需要性能強(qiáng)大、計(jì)算速度較快的計(jì)算機(jī)進(jìn)行實(shí)現(xiàn)，再加上在實(shí)際應(yīng)用中需要高性能的處理器來(lái)部署算法，增加了工程應(yīng)用的成本。因此本文提出的輪椅目標(biāo)檢測(cè)與跟蹤算法，首先利用目標(biāo)檢測(cè)算法對(duì)目標(biāo)進(jìn)行初始化，當(dāng)檢測(cè)到目標(biāo)后目標(biāo)檢測(cè)算法自動(dòng)失效，再啟動(dòng)目標(biāo)跟蹤算法，當(dāng)目標(biāo)長(zhǎng)時(shí)間丟失時(shí)跟蹤算法失效，這時(shí)又重新啟動(dòng)目標(biāo)檢測(cè)算法，這樣既避免了目標(biāo)的丟失，又避免了手動(dòng)初始化搜索窗口的弊端。

1.1 自適應(yīng)高斯混合建模的背景減除法

背景減除法[10]是一種通過(guò)數(shù)學(xué)模型，先建立背景模型，然后再進(jìn)行差分運(yùn)算，最后完成目標(biāo)檢測(cè)的方法，因此背景模型的好壞決定了目標(biāo)檢測(cè)能否準(zhǔn)確。高斯混合模型（Gaussian Μixture Μodel，GΜΜ）是構(gòu)建背景模型常用的方法之一。高斯混合模型通過(guò)對(duì)各像素點(diǎn)的顏色值變化與高斯概率分布的匹配情況，完成對(duì)權(quán)重、均值和協(xié)方差等參數(shù)的更新，使得背景像素值服從某幾個(gè)高斯分布函數(shù)，實(shí)現(xiàn)聚類(lèi)分布，從而實(shí)現(xiàn)對(duì)背景的建模[6]。

高斯混合模型在參數(shù)更新時(shí)，首先對(duì)各像素點(diǎn)均值以隨機(jī)數(shù)0～255 進(jìn)行初始化，賦予各像素點(diǎn)權(quán)值w=1/k，通過(guò)判斷圖像變化后的像素值xt與高斯概率分布的匹配情況，進(jìn)行相應(yīng)的參數(shù)更新。匹配判斷的條件為：

式（1）中，當(dāng)D 取2.5時(shí)一般效果最好，σi,t-1為協(xié)方差，μi,t-1為均值。

如果得到的分布能夠匹配，則對(duì)權(quán)值參數(shù)以如下規(guī)則更新：

式（2）中，α 為權(quán)值的學(xué)習(xí)速率，0 ＜α ＜1；ρ 為參數(shù)更新速率，如式（5）所示：

其中，t 時(shí)刻第j 個(gè)高斯分布函數(shù)η，如式（6）所示：

如果得到的分布部分無(wú)法匹配，則保持均值和標(biāo)準(zhǔn)差的值不變，其權(quán)重更新方式如式（7）所示：

當(dāng)分布均未達(dá)到匹配條件時(shí)，對(duì)wi/σi進(jìn)行替換，賦權(quán)重較小值w=1/10，標(biāo)準(zhǔn)差σ=6。

當(dāng)參數(shù)更新完成后，以ri=wi/σi進(jìn)行排序，并歸一化權(quán)值，之后對(duì)當(dāng)前幀和構(gòu)建的背景模型進(jìn)行差分運(yùn)算，得到前景目標(biāo)[11]。

混合高斯模型通過(guò)對(duì)每個(gè)像素構(gòu)造多個(gè)模型，可以對(duì)復(fù)雜情況下的背景模型實(shí)現(xiàn)有效的構(gòu)建，但該算法對(duì)構(gòu)建模型的時(shí)間消耗過(guò)長(zhǎng)，并且學(xué)習(xí)率α 設(shè)置的不同將直接影響目標(biāo)的檢測(cè)結(jié)果。同時(shí)α 的設(shè)置需要根據(jù)背景復(fù)雜度的不同進(jìn)行手動(dòng)調(diào)整，而自適應(yīng)目標(biāo)檢測(cè)算法能夠根據(jù)背景的變換，實(shí)時(shí)更新背景模型，以適應(yīng)外部環(huán)境引起的變化，不需要手動(dòng)調(diào)整學(xué)習(xí)率α，因此將兩者進(jìn)行結(jié)合，提出了自適應(yīng)高斯混合建模的背景減除法。針對(duì)高斯混合模型存在的問(wèn)題進(jìn)行如下的改進(jìn)：

（1）引入?yún)?shù)ck來(lái)記錄匹配第k 個(gè)高斯分量的觀測(cè)值個(gè)數(shù)，高斯分布個(gè)數(shù)為k。

首先，采用包含最大后驗(yàn)概率P( ηk|xk)的高斯分布權(quán)值，利用迭代的方法對(duì)權(quán)重進(jìn)行更新：

然后，利用qk( xt)對(duì)觀測(cè)值以及高斯分布函數(shù)進(jìn)行更新：

最后，完成對(duì)均值和協(xié)方差的更新：

其中，ηk為高斯分布函數(shù)，σk為協(xié)方差，μk為均值，α為權(quán)值的學(xué)習(xí)速率。

為了驗(yàn)證本文提出的算法，分別對(duì)本文算法，以及將學(xué)習(xí)率設(shè)置為0.05 和0.20 時(shí)的高斯混合模型的背景減除法分別進(jìn)行仿真，并且對(duì)仿真結(jié)果進(jìn)行對(duì)比分析，仿真結(jié)果如圖1～圖3所示。

圖1 學(xué)習(xí)率為0.05時(shí)高斯混合算法處理結(jié)果

圖2 學(xué)習(xí)率為0.20時(shí)高斯混合算法處理結(jié)果

圖3 自適應(yīng)高斯混合模型算法處理結(jié)果

由圖1～圖3 可以看出，當(dāng)學(xué)習(xí)率設(shè)置為0.20 時(shí)，高斯混合背景建模的背景減除法構(gòu)建的背景相比于在學(xué)習(xí)率0.05時(shí)更加清晰，但得到的目標(biāo)缺失了局部邊緣重要的信息，造成目標(biāo)檢測(cè)的結(jié)果不佳。而自適應(yīng)高斯混合建模的背景減除法所構(gòu)建的背景更加清晰，包含的信息更加豐富，且背景不存在當(dāng)前的目標(biāo)殘影，使得檢測(cè)到的運(yùn)動(dòng)目標(biāo)輪廓更加清晰。

改進(jìn)算法與原算法對(duì)圖像處理的時(shí)間對(duì)比如表1所示，改進(jìn)算法相比于高斯混合背景建模的背景減除法，不需要根據(jù)環(huán)境的不同設(shè)置學(xué)習(xí)率，且處理速度有一定的提高。

表1 改進(jìn)算法與高斯混合算法處理時(shí)間對(duì)比

1.2 基于粒子濾波的TLD跟蹤算法

TLD 跟蹤算法把傳統(tǒng)的目標(biāo)跟蹤算法與檢測(cè)算法相結(jié)合，并通過(guò)學(xué)習(xí)模塊來(lái)對(duì)跟蹤模塊中的結(jié)果進(jìn)行評(píng)估，從而對(duì)檢測(cè)模塊中的分類(lèi)器不斷更新，實(shí)現(xiàn)了對(duì)目標(biāo)的長(zhǎng)時(shí)間跟蹤，并能夠在一定程度上克服目標(biāo)姿態(tài)的變化問(wèn)題[12]。TLD算法由跟蹤器、學(xué)習(xí)器和檢測(cè)器三部分構(gòu)成，但其跟蹤模塊使用LΚ 光流法采用單一的灰度特征對(duì)目標(biāo)進(jìn)行跟蹤，易受到光照干擾和遮擋因素的影響；并且檢測(cè)器在掃描檢測(cè)的過(guò)程中，會(huì)計(jì)算大量與目標(biāo)無(wú)關(guān)的信息，導(dǎo)致計(jì)算量增大，使檢測(cè)模塊的計(jì)算速度變慢[13]。

針對(duì)上述問(wèn)題，提出了一種基于多特征融合的粒子濾波算法與TLD算法相結(jié)合的新算法來(lái)對(duì)目標(biāo)實(shí)現(xiàn)跟蹤，減少了TLD 算法中單一灰度特征在背景復(fù)雜和有相似目標(biāo)情況下的干擾，同時(shí)融合馬爾科夫模型的方向預(yù)判算法對(duì)檢測(cè)模塊進(jìn)行改進(jìn)，減少了對(duì)掃描窗中大量不包含目標(biāo)的計(jì)算，從而避免了不必要的大量計(jì)算對(duì)資源的消耗，提高了檢測(cè)模塊的計(jì)算速度。算法的總體架構(gòu)如圖4所示。

圖4 改進(jìn)的TLD算法結(jié)構(gòu)框圖

本文的改進(jìn)算法將粒子濾波跟蹤算法代替TLD跟蹤算法中的跟蹤器，原先的TLD 跟蹤算法中的跟蹤器只采用單一灰度特征難以對(duì)目標(biāo)形成穩(wěn)定的跟蹤，因此本文采用的方法是將目標(biāo)的顏色特征、紋理特征和HOG特征進(jìn)行融合，利用多特征融合完成對(duì)目標(biāo)的穩(wěn)定跟蹤。首先使用表示目標(biāo)與候選區(qū)域相似性的Bhattacharyya系數(shù)對(duì)目標(biāo)的顏色特征、紋理特征和HOG 特征進(jìn)行各自似然函數(shù)求解，公式如下：

其中，ρ 為Bhattacharyya式的系數(shù)，系數(shù)越大，說(shuō)明目標(biāo)模型與候選目標(biāo)的相似度較大。將式（14）代入似然函數(shù)得到下式：

其中，Mi-1表示要跟蹤的目標(biāo)，即上一幀的跟蹤目標(biāo)，Hi表示此時(shí)的候選目標(biāo)。當(dāng)式（14）似然函數(shù)計(jì)算的結(jié)果越大時(shí)，說(shuō)明目標(biāo)Mi-1與候選目標(biāo)Hi的相似性較大。同理可得三種特征分量的似然函數(shù)如下：

在場(chǎng)景變換較大的目標(biāo)跟蹤過(guò)程中，對(duì)目標(biāo)與背景差異較大的特征賦予較高的權(quán)重，使用權(quán)重較大的特征對(duì)目標(biāo)進(jìn)行跟蹤的可靠性更大。

因此，在跟蹤過(guò)程中，隨著場(chǎng)景的變化，要對(duì)三種特征定義多特征集F=( f|f ∈( c,t,h ))，計(jì)算不同特征下目標(biāo)模型與候選目標(biāo)模型的相似度以及與目標(biāo)和背景的相似度，融合權(quán)重wf，進(jìn)而得到總的目標(biāo)的相似性，如式（17）所示：

利用特征區(qū)分度因子定義歸一化權(quán)重為：

以單變量非線性系統(tǒng)為模型，完成仿真測(cè)試，如式（21）所示，粒子初值x0=0.1,迭代次數(shù)t=100，粒子數(shù)N=500，過(guò)程噪聲Vk和觀測(cè)噪聲Wk均值和方差設(shè)為0和1：

算法仿真對(duì)比圖如圖5 所示。圖5（a）為單一特征粒子濾波算法和多特征融合粒子濾波算法的均方根誤差對(duì)比，仿真結(jié)果表明，多特征融合的粒子濾波的均方根誤差更??；圖5（b）為粒子濾波的狀態(tài)估計(jì)值與真實(shí)值的比較，與基于單一特征的粒子濾波算法相比，多特征融合的粒子濾波算法的狀態(tài)估計(jì)值更接近于真實(shí)狀態(tài)，與系統(tǒng)的擬合效果更好，仿真結(jié)果說(shuō)明多特征融合的粒子濾波算法精度更高。

圖5 算法仿真結(jié)果對(duì)比

在將粒子濾波跟蹤算法代替TLD跟蹤算法中的跟蹤器后，同時(shí)改進(jìn)TLD的檢測(cè)模塊，在檢測(cè)模塊中融合馬爾科夫模型方向預(yù)判算法，能夠根據(jù)上一幀目標(biāo)的運(yùn)動(dòng)方向，預(yù)測(cè)出當(dāng)前幀目標(biāo)可能出現(xiàn)的區(qū)域，并選取合適的掃描窗對(duì)目標(biāo)可能出現(xiàn)的區(qū)域進(jìn)行檢測(cè)，提高檢測(cè)模塊的工作效率。因此本文改進(jìn)算法的基本步驟如下：

（3）計(jì)算特征權(quán)重。根據(jù)當(dāng)前粒子分布，利用式（22）狀態(tài)轉(zhuǎn)移矩陣，可計(jì)算得到特征的權(quán)重，并歸一化。

其中，R( St=1) 表示在t 時(shí)刻目標(biāo)向右運(yùn)動(dòng)的概率，R( St=-1) 表示在t 時(shí)刻目標(biāo)向左運(yùn)動(dòng)的概率。如果在t 時(shí)刻目標(biāo)向右運(yùn)動(dòng)，則R( St=-1) =0；如果在t 時(shí)刻目標(biāo)不發(fā)生運(yùn)動(dòng)，則R( St=-1) =0.5。

（4）進(jìn)行粒子重采樣，估計(jì)待跟蹤的目標(biāo)當(dāng)前幀的狀態(tài)。

（5）使用上一步產(chǎn)生的正負(fù)樣本來(lái)構(gòu)建目標(biāo)模型，初始化跟蹤器和級(jí)聯(lián)分類(lèi)器并進(jìn)行訓(xùn)練。

（6）利用改進(jìn)的檢測(cè)模塊中具有方向預(yù)測(cè)的掃描窗口對(duì)可能目標(biāo)的窗口進(jìn)行掃描，確定可能包含目標(biāo)的掃描窗。

（7）綜合判斷跟蹤器和檢測(cè)器的結(jié)果。判斷目標(biāo)在當(dāng)前幀中是否被成功跟蹤，對(duì)目標(biāo)模型進(jìn)行在線更新并判斷是否需要對(duì)獲得的正負(fù)樣本進(jìn)行學(xué)習(xí)。

（8）學(xué)習(xí)器分類(lèi)訓(xùn)練結(jié)果。若綜合模塊認(rèn)為需要對(duì)檢測(cè)器進(jìn)行進(jìn)一步訓(xùn)練，則學(xué)習(xí)器利用新的正負(fù)訓(xùn)練樣本對(duì)級(jí)聯(lián)分類(lèi)器訓(xùn)練并更新相關(guān)參數(shù)。

（9）顯示當(dāng)前幀的跟蹤結(jié)果，繼續(xù)執(zhí)行（2）～（8）的步驟直到跟蹤結(jié)束。

在實(shí)際應(yīng)用中，目標(biāo)可能處于復(fù)雜多變的環(huán)境中，如圖6 所示，在室外場(chǎng)景存在光照變化、樹(shù)葉擺動(dòng)以及行人的復(fù)雜情況下，對(duì)本文提出的目標(biāo)跟蹤算法與粒子濾波算法以及TLD算法進(jìn)行仿真對(duì)比。實(shí)驗(yàn)中粒子數(shù)目為100，設(shè)置優(yōu)先級(jí)閾值為0.95。

三種算法在剛開(kāi)始存在光照的情況下都能跟蹤到目標(biāo)，如圖6（a）所示；但當(dāng)目標(biāo)開(kāi)始前進(jìn)，迎面過(guò)來(lái)行人干擾且發(fā)生局部遮擋的情況下，其他兩種算法對(duì)目標(biāo)的跟蹤發(fā)生了漂移現(xiàn)象，產(chǎn)生大量偏移目標(biāo)的粒子，導(dǎo)致目標(biāo)跟蹤的準(zhǔn)確性下降，而本文采用的算法依舊能夠跟蹤到目標(biāo)，如圖6（b）和圖6（c）所示；在光照發(fā)生變化與目標(biāo)全部遮擋的情形下，本文提出的算法采用多特征融合與馬爾科夫預(yù)測(cè)方法能夠?qū)崿F(xiàn)對(duì)光照變化和遮擋情況下的目標(biāo)跟蹤，而其他兩種算法在目標(biāo)受到完全遮擋的情況下，粒子分布過(guò)于分散，因此發(fā)生了目標(biāo)丟失問(wèn)題，導(dǎo)致目標(biāo)跟蹤失敗，如圖6（d）和圖6（e）所示。

圖6 本文算法與其他算法在復(fù)雜情況下仿真對(duì)比圖

如圖7 所示，圖中數(shù)據(jù)為通過(guò)隨機(jī)采樣獲得的100個(gè)粒子中部分粒子的坐標(biāo)值以及權(quán)重值，權(quán)重的范圍為0.982 4～0.991 8（圖中為經(jīng)過(guò)歸一化處理后的數(shù)值，真實(shí)數(shù)值需要再乘上粒子的總數(shù)N=100），可以看出權(quán)重都大于設(shè)定的閾值0.95，保證了粒子濾波在跟蹤目標(biāo)特征時(shí)的準(zhǔn)確性。

圖7 粒子坐標(biāo)和權(quán)重的采樣值

如圖8所示，為了更清晰直觀地看出跟蹤算法在目標(biāo)跟蹤過(guò)程中的準(zhǔn)確性，繪制三種算法在復(fù)雜情況下的跟蹤重疊率曲線對(duì)比圖。從圖中可以看出，在第10 幀到25幀和第37幀到45幀的跟蹤中，由于發(fā)生了目標(biāo)的遮擋和光照的變化，其他兩種算法在目標(biāo)跟蹤時(shí)重疊率迅速下降導(dǎo)致目標(biāo)丟失，跟蹤失敗。此時(shí)，本文提出的算法目標(biāo)跟蹤的重疊率雖然有所下降，但依舊保持在0.67 左右，說(shuō)明在目標(biāo)遮擋和光照變化的情況下，能夠?qū)崿F(xiàn)對(duì)目標(biāo)的跟蹤，跟蹤效果較好。

圖8 復(fù)雜情況下的重疊率曲線對(duì)比圖

由以上仿真結(jié)果可以看出，本文提出的算法在單一目標(biāo)跟蹤中具有較好的跟蹤精度和魯棒性，能夠有效解決目標(biāo)形變、復(fù)雜背景干擾和遮擋等問(wèn)題，當(dāng)目標(biāo)消失后再次出現(xiàn)在視野中時(shí)，能夠快速準(zhǔn)確地鎖定目標(biāo)，實(shí)現(xiàn)再次跟蹤，且目標(biāo)跟蹤重疊率基本保持在0.7左右，能夠滿(mǎn)足跟蹤要求。

1.3 雙目視覺(jué)定位

雙目立體視覺(jué)定位的原理與人類(lèi)雙眼定位原理相似，就是利用參數(shù)相同的左右相機(jī)拍攝同一物體不同角度的立體圖像，然后使用三角測(cè)量原理計(jì)算出左右圖像對(duì)比匹配點(diǎn)的視覺(jué)差，從而計(jì)算出特征點(diǎn)的三維坐標(biāo)，實(shí)現(xiàn)對(duì)物體的測(cè)距定位[14]，如圖9所示為雙目相機(jī)模型，左攝像機(jī)和右攝像機(jī)參數(shù)相同，它們的基線距為B，焦距為f ，空間中的特征點(diǎn)P 在兩個(gè)相機(jī)上成像，其中Pl=( Xl,Yl)與Pr=( Xr,Yr)分別是點(diǎn)P 在左右攝像頭的圖像坐標(biāo)系中的坐標(biāo)，P( xc,yc,zc)為點(diǎn)P 在世界坐標(biāo)系下的坐標(biāo)。理想情況下特征點(diǎn)P 在左右相機(jī)圖像下的水平坐標(biāo)是相等的，在圖9中，根據(jù)相似三角形原理，可得：

假設(shè)某一點(diǎn)在左右攝像頭中的視覺(jué)差為Ds=xl-xr，則上式可轉(zhuǎn)換為：

根據(jù)式（24）可以得到特征點(diǎn)空間中的三維坐標(biāo)( xc,yc,zc)，利用雙目相機(jī)模型參數(shù)將其代入到定位的坐標(biāo)中完成坐標(biāo)系轉(zhuǎn)換，計(jì)算出空間中的三維坐標(biāo)。

圖9 雙目立體成像平面示意圖

假設(shè)左攝像機(jī)所在的坐標(biāo)系與世界坐標(biāo)( XW,YW,ZW)重合，Ol-XlYl表示左圖像坐標(biāo)系，焦距為fl；or-xryrzr表示右攝像機(jī)物理坐標(biāo)系，同理Or-XrYr表示右圖像坐標(biāo)系，焦距為fr；根據(jù)攝像機(jī)透視投影，得到如下關(guān)系式，其中sl、sr分別為左右相機(jī)的畸變矩陣參數(shù)：

世界坐標(biāo)系與攝像機(jī)坐標(biāo)之間的空間轉(zhuǎn)換關(guān)系可表示為式（27）：

其中，Mlr=[ R|T ]，表示左右兩個(gè)相機(jī)坐標(biāo)系之間的空間旋轉(zhuǎn)矩陣和平移向量。

同理，根據(jù)式（25）、式（26）、式（27），對(duì)于世界坐標(biāo)系( XW,YW,ZW)中的空間點(diǎn)，左右相機(jī)透視投影點(diǎn)之間的對(duì)應(yīng)關(guān)系為：

因此，三維空間中某一點(diǎn)的坐標(biāo)可表示為：

由此得到目標(biāo)位置坐標(biāo)，并能夠計(jì)算出目標(biāo)相對(duì)于輪椅的夾角為cos θ=XW/ZW。

在完成對(duì)目標(biāo)的測(cè)距定位，從而確定目標(biāo)相對(duì)于輪椅的位置后，可以通過(guò)獲取目標(biāo)的位置距離信息來(lái)調(diào)整輪椅的姿態(tài)方向和速度。本文使用比例控制方法，根據(jù)目標(biāo)與輪椅的跟隨距離來(lái)調(diào)節(jié)直流電機(jī)在跟隨中的速度。將跟隨距離劃分為三個(gè)區(qū)域，即加速區(qū)、減速區(qū)與制動(dòng)區(qū)，其跟隨速度控制結(jié)構(gòu)圖如圖10 所示。當(dāng)輪椅對(duì)目標(biāo)的跟隨距離dk大于跟隨設(shè)定距離db時(shí)，輪椅加速跟隨。當(dāng)輪椅跟隨距離dk等于跟蹤設(shè)定距離db時(shí)，輪椅以速度vr穩(wěn)速跟隨。當(dāng)輪椅跟隨距離dk大于ds且小于db時(shí)，輪椅減速跟隨。當(dāng)輪椅跟隨距離dk小于最小跟蹤距離ds時(shí)，表明輪椅已到達(dá)最小安全距離，此時(shí)輪椅需要停止運(yùn)動(dòng)等待跟隨。

圖10 跟隨速度控制結(jié)構(gòu)圖

因此，輪椅的跟隨速度控制可表示為式（30）：

其中，kp1、kp2為負(fù)常數(shù)，Δd 為距離誤差。

在系統(tǒng)中，設(shè)置輪椅跟隨距離為150 cm，最小安全距離為80 cm，穩(wěn)定跟蹤時(shí)目標(biāo)的速度設(shè)vr為0.5 m/s，則輪椅的速度控制為：

最后通過(guò)獲取目標(biāo)的位置距離信息來(lái)調(diào)整輪椅的姿態(tài)方向和速度，實(shí)現(xiàn)對(duì)目標(biāo)的準(zhǔn)確跟蹤。

2 系統(tǒng)總體方案設(shè)計(jì)

圖11 系統(tǒng)整體設(shè)計(jì)框圖

本文主要針對(duì)輪椅的跟隨功能進(jìn)行研究與開(kāi)發(fā)，整個(gè)系統(tǒng)包含五部分：控制部分、傳感器部分、執(zhí)行部分、遠(yuǎn)程調(diào)試單元和鋰電池供電部分。如圖11 所示，系統(tǒng)的主要工作過(guò)程為：鋰電池供電部分為整個(gè)智能輪椅系統(tǒng)提供電源，保證系統(tǒng)持續(xù)、穩(wěn)定地工作。傳感器部分中的視覺(jué)傳感器檢測(cè)周?chē)h(huán)境和特定目標(biāo)，提取目標(biāo)的特征，進(jìn)而確定目標(biāo)的位置信息；激光傳感器對(duì)障礙物和視覺(jué)傳感器存在的盲區(qū)的障礙物進(jìn)行檢測(cè)，同時(shí)姿態(tài)傳感器作為輪椅的方位感知元件，在輪椅的跟隨過(guò)程中，實(shí)時(shí)感知輪椅的方向姿態(tài)；GPS 傳感器實(shí)時(shí)確定輪椅的世界坐標(biāo)，通過(guò)各個(gè)傳感器實(shí)現(xiàn)對(duì)目標(biāo)的位置信息，輪椅的世界坐標(biāo)、方位姿態(tài)以及障礙物等信息的采集，并實(shí)時(shí)將所有的數(shù)據(jù)信息發(fā)送給輪椅控制器[15]?？刂破魍ㄟ^(guò)對(duì)數(shù)據(jù)信息的讀取和相應(yīng)的算法處理之后，發(fā)送執(zhí)行命令給執(zhí)行機(jī)構(gòu)。執(zhí)行機(jī)構(gòu)通過(guò)對(duì)左右電機(jī)的控制，實(shí)時(shí)控制輪椅的運(yùn)動(dòng)狀態(tài)，從而實(shí)現(xiàn)輪椅對(duì)目標(biāo)自動(dòng)跟隨與避障的功能。遠(yuǎn)程調(diào)試單元用于實(shí)現(xiàn)對(duì)該系統(tǒng)控制部分的處理器和控制器進(jìn)行遠(yuǎn)程編程和調(diào)試。

2.1 系統(tǒng)硬件設(shè)計(jì)

本文設(shè)計(jì)的輪椅跟隨系統(tǒng)硬件主要分為下位機(jī)和上位機(jī)兩部分。

上位機(jī)系統(tǒng)由樹(shù)莓派控制器、雙目攝像頭和激光雷達(dá)傳感器組成。上位機(jī)控制器采用樹(shù)莓派4 代B 版（Raspberry Pi4.0），利用樹(shù)莓派自帶的WIFI 模塊可以將樹(shù)莓派與電腦PC端利用遠(yuǎn)程調(diào)試單元連接起來(lái)。如圖12所示為樹(shù)莓派調(diào)試界面。

圖12 樹(shù)莓派調(diào)試界面

下位機(jī)系統(tǒng)由STΜ32控制器、電機(jī)驅(qū)動(dòng)電路、姿態(tài)傳感器和GPS傳感器組成，系統(tǒng)硬件框圖如圖13所示。

圖13 系統(tǒng)硬件框圖

輪椅跟隨系統(tǒng)要完成對(duì)目標(biāo)的跟蹤與避障功能，首先要通過(guò)攝像頭獲取目標(biāo)的圖像信息，通過(guò)USB 串口與上位機(jī)進(jìn)行數(shù)據(jù)交互，同時(shí)激光雷達(dá)采集輪椅周?chē)沫h(huán)境信息，也通過(guò)USB 串口通訊發(fā)送給上位機(jī)進(jìn)行數(shù)據(jù)處理。上位機(jī)獲取到視頻圖像信息數(shù)據(jù)之后，經(jīng)過(guò)目標(biāo)檢測(cè)、目標(biāo)跟蹤算法以及雙目定位算法處理后得到目標(biāo)的相應(yīng)位置信息數(shù)據(jù)，經(jīng)過(guò)串口與下位機(jī)進(jìn)行通信[16]。與此同時(shí)，下位機(jī)STΜ32通過(guò)I/O口實(shí)時(shí)獲取GPS 傳感器和姿態(tài)傳感器發(fā)送的世界坐標(biāo)數(shù)據(jù)信息和輪椅的姿態(tài)信息，下位機(jī)根據(jù)獲得的目標(biāo)位置數(shù)據(jù)計(jì)算出相應(yīng)的PWΜ波控制電機(jī)驅(qū)動(dòng)電路，從而控制電機(jī)的運(yùn)動(dòng)狀態(tài)，實(shí)現(xiàn)對(duì)目標(biāo)的跟隨。同時(shí)電機(jī)編碼器測(cè)得電機(jī)的實(shí)時(shí)速度數(shù)據(jù)通過(guò)中斷發(fā)送給下位機(jī)，獲得輪椅的實(shí)際轉(zhuǎn)速，通過(guò)運(yùn)動(dòng)控制算法實(shí)現(xiàn)閉環(huán)反饋。在跟隨過(guò)程中，根據(jù)激光傳感器檢測(cè)的障礙物信息進(jìn)行避障，在避障完成后根據(jù)姿態(tài)傳感器檢測(cè)的輪椅姿態(tài)方向變化數(shù)據(jù)對(duì)輪椅的姿態(tài)進(jìn)行調(diào)整。在跟蹤過(guò)程中不斷重復(fù)運(yùn)行上述階段，直至完成跟蹤任務(wù)或被強(qiáng)制關(guān)閉停止[17]。

2.2 系統(tǒng)軟件設(shè)計(jì)

輪椅跟隨控制部分是整個(gè)系統(tǒng)的核心部分，其主程序流程圖如圖14所示，當(dāng)有監(jiān)護(hù)人員陪同時(shí)，可以將輪椅從自動(dòng)跟隨模式切換為手動(dòng)控制模式。工作過(guò)程主要如下：

（1）對(duì)系統(tǒng)初始化。

（2）調(diào)用GPS 定位子程序，并將輪椅的位置信息發(fā)送到手機(jī)微信小程序中，使監(jiān)護(hù)人可以及時(shí)查看輪椅所處的位置。

（3）選擇模式，即跟隨模式或手動(dòng)控制模式。

（4）若為跟隨模式，調(diào)用目標(biāo)檢測(cè)與跟蹤子程序，感知輪椅前方要跟隨的目標(biāo)人物，判斷是否有運(yùn)動(dòng)目標(biāo)，實(shí)現(xiàn)目標(biāo)檢測(cè)功能。當(dāng)檢測(cè)到目標(biāo)人物后，根據(jù)感知信息獲取跟蹤目標(biāo)的目標(biāo)框位置信息，并將該信息發(fā)送到目標(biāo)跟蹤模塊。

（5）調(diào)用雙目測(cè)距定位算法，對(duì)目標(biāo)的位置進(jìn)行定位，確定目標(biāo)相對(duì)于輪椅的方向和距離。

（6）在跟隨過(guò)程中，調(diào)用跟隨與避障運(yùn)動(dòng)控制程序，執(zhí)行相應(yīng)的跟隨與避障操作。

圖14 系統(tǒng)主程序流程圖

（7）判斷跟隨模式是否結(jié)束，若結(jié)束，則整個(gè)主程序停止運(yùn)行，若沒(méi)有結(jié)束，則返回繼續(xù)執(zhí)行步驟（3）～（7），直到跟隨任務(wù)停止。

3 系統(tǒng)測(cè)試及分析

自動(dòng)跟隨輪椅的整體實(shí)物圖如圖15 所示，筆者在搭建完實(shí)驗(yàn)平臺(tái)后，對(duì)自動(dòng)跟隨輪椅的功能進(jìn)行了測(cè)試，包括室內(nèi)場(chǎng)景中單一目標(biāo)的跟隨避障測(cè)試、室外場(chǎng)景中存在人員干擾時(shí)的跟隨避障測(cè)試以及GPS 的定位測(cè)試。

圖15 輪椅跟隨系統(tǒng)的整體實(shí)物圖

3.1 靜態(tài)環(huán)境中的跟隨與避障測(cè)試

為了測(cè)試輪椅跟隨系統(tǒng)的跟隨和避障功能的效果，在室內(nèi)環(huán)境進(jìn)行跟隨與避障測(cè)試，如圖16所示。圖16（a）～（d）展示了從輪椅啟動(dòng)階段開(kāi)始先加速前進(jìn)，再穩(wěn)定跟隨，之后減速避障，最后加速跟隨的過(guò)程，利用1.3 節(jié)中的電機(jī)調(diào)速方法，實(shí)現(xiàn)了根據(jù)目標(biāo)的位置來(lái)控制輪椅的速度和方向，使輪椅與目標(biāo)人物保持設(shè)定的距離和相對(duì)方位的要求。圖16（e）～（h）則展示了利用本文提出的目標(biāo)檢測(cè)和目標(biāo)跟蹤算法，實(shí)現(xiàn)對(duì)目標(biāo)人物提取和追蹤的過(guò)程。目標(biāo)人員在行走的同時(shí)，利用樹(shù)莓派的遠(yuǎn)程調(diào)試單元記錄跟隨過(guò)程的距離偏差，如圖17 所示。目標(biāo)期望位置為輪椅前方150 cm處，設(shè)置數(shù)據(jù)采集間隔為1 s。從圖17中可以看出，在輪椅啟動(dòng)階段，系統(tǒng)正在對(duì)目標(biāo)進(jìn)行檢測(cè)與處理，檢測(cè)識(shí)別的時(shí)間大概為100 ms，與此同時(shí)輪椅與目標(biāo)的設(shè)定的跟隨距離的偏差迅速增大；在開(kāi)始跟隨后，輪椅開(kāi)始加速前進(jìn)，速度增大，跟隨距離的偏差開(kāi)始減??；處于穩(wěn)定跟隨階段時(shí)，跟隨的偏差基本保持在10 cm 左右；當(dāng)遇到障礙物進(jìn)行避障操作時(shí)，輪椅的跟隨距離偏差也開(kāi)始逐漸增加，避開(kāi)障礙物后，繼續(xù)進(jìn)行跟隨?？梢钥闯霰疚脑O(shè)計(jì)的系統(tǒng)在室內(nèi)靜態(tài)環(huán)境中能夠?qū)崿F(xiàn)對(duì)目標(biāo)的跟隨，并且在跟隨過(guò)程中安全地躲避障礙物，滿(mǎn)足實(shí)際場(chǎng)景中的跟隨要求。

圖16 室內(nèi)跟隨與避障測(cè)試過(guò)程

圖17 靜態(tài)環(huán)境跟隨距離偏差數(shù)據(jù)

3.2 目標(biāo)干擾和丟失情況下的跟隨與避障測(cè)試

在實(shí)際的應(yīng)用中，輪椅是針對(duì)所有用戶(hù)使用的，因此本次實(shí)驗(yàn)重新設(shè)置目標(biāo)人員來(lái)檢測(cè)系統(tǒng)復(fù)位之后的識(shí)別能力，且設(shè)置的測(cè)試環(huán)境在同一時(shí)間存在目標(biāo)干擾、避障以及避障之后目標(biāo)丟失的情況。實(shí)驗(yàn)過(guò)程如圖18所示。

圖18 目標(biāo)干擾和丟失情況下的跟隨實(shí)驗(yàn)

測(cè)試結(jié)果如圖18所示。結(jié)合圖19復(fù)雜情況下的跟隨距離偏差可以看出，圖18（a）為輪椅重新復(fù)位后選擇要跟隨的目標(biāo)人物，重新檢測(cè)目標(biāo)的時(shí)間大約需要14 s，此時(shí)輪椅與目標(biāo)的跟隨偏差距離逐漸開(kāi)始增大，且增大到21 cm，隨后檢測(cè)到目標(biāo)開(kāi)始跟隨，輪椅加速前進(jìn)，跟隨距離偏差逐漸減??；圖16（b）～（h）為輪椅在跟隨過(guò)程中遇到狹窄空間、出現(xiàn)干擾人員以及目標(biāo)左轉(zhuǎn)之后的丟失情況，輪椅在干擾人員出現(xiàn)后依舊能夠準(zhǔn)確地跟隨目標(biāo)人員，當(dāng)目標(biāo)開(kāi)始左轉(zhuǎn)出門(mén)，消失在輪椅的視覺(jué)范圍之內(nèi)時(shí)，本文所提出的跟隨算法能夠?qū)崿F(xiàn)目標(biāo)的位置預(yù)測(cè)，實(shí)現(xiàn)準(zhǔn)確的左轉(zhuǎn)跟隨且安全地避開(kāi)墻壁障礙物，在此階段跟隨距離偏差開(kāi)始波動(dòng)并且增加，隨后開(kāi)始減小并趨于穩(wěn)定。

圖19 目標(biāo)干擾和丟失情況下的跟隨距離偏差

3.3 GPS定位測(cè)試

在輪椅跟隨系統(tǒng)中加入GPS 定位模塊的主要作用是防止在發(fā)生意外的情況下，輪椅使用者的監(jiān)護(hù)人或看護(hù)人員無(wú)法聯(lián)系到使用者，通過(guò)GPS定位模塊發(fā)送的位置信息，就可以確定輪椅此時(shí)的位置，從而找到使用者。

如圖20所示為樹(shù)莓派遠(yuǎn)程桌面輸出的輪椅經(jīng)緯度坐標(biāo)位置數(shù)據(jù)，圖中顯示了從GPS定位模塊中采集到的位置數(shù)據(jù)。

圖20 GPS定位模塊數(shù)據(jù)采集結(jié)果

得到的位置信息即經(jīng)緯度，需要將其上傳到ΜQTT服務(wù)器，如圖21所示。對(duì)服務(wù)器的數(shù)據(jù)進(jìn)行封裝，發(fā)布Topic主題，經(jīng)過(guò)后臺(tái)ΜQTT服務(wù)器接收定位數(shù)據(jù)，使用騰訊地圖的開(kāi)發(fā)服務(wù)將實(shí)時(shí)的經(jīng)緯度轉(zhuǎn)換為地圖上的地址坐標(biāo)，再通過(guò)微信小程序進(jìn)行顯示和查看。

微信小程序的定位結(jié)果如圖22所示。從圖22中可以看出此時(shí)輪椅所在的位置，經(jīng)過(guò)測(cè)試表明GPS定位模塊能夠?qū)崟r(shí)地發(fā)送輪椅的位置坐標(biāo)，雖然跟實(shí)際的坐標(biāo)有一定的誤差，但這種誤差不影響輪椅用戶(hù)的家人或看護(hù)人員實(shí)時(shí)查看其位置，方便在長(zhǎng)時(shí)間聯(lián)系不到輪椅用戶(hù)時(shí)，確定輪椅的位置，進(jìn)而找到輪椅用戶(hù)，滿(mǎn)足系統(tǒng)的基本要求。

圖21 ΜQTT服務(wù)器界面

圖22 小程序定位結(jié)果

4 結(jié)束語(yǔ)

本文設(shè)計(jì)了一種基于計(jì)算機(jī)視覺(jué)的自動(dòng)跟隨輪椅的目標(biāo)跟隨系統(tǒng)，先利用自適應(yīng)高斯混合建模的背景減除法檢測(cè)到目標(biāo)，再利用基于自適應(yīng)多特征融合粒子濾波的TLD 目標(biāo)跟蹤算法在目標(biāo)受到遮擋、背景干擾和目標(biāo)短時(shí)間丟失的情況下仍能實(shí)現(xiàn)對(duì)目標(biāo)的穩(wěn)定跟蹤。在實(shí)驗(yàn)室環(huán)境下對(duì)各個(gè)功能進(jìn)行了測(cè)試驗(yàn)證，包括單人跟隨測(cè)試，目標(biāo)干擾情況下的跟隨測(cè)試以及定位測(cè)試，聯(lián)調(diào)測(cè)試結(jié)果表明，本文設(shè)計(jì)的輪椅系統(tǒng)對(duì)目標(biāo)人員的跟隨效果良好，在跟隨過(guò)程中能夠及時(shí)地避開(kāi)障礙物，測(cè)試結(jié)果較為穩(wěn)定。