為障礙或特征發(fā)現(xiàn) 裝置，由此在檢測到的障礙或特征在水池清潔器12正前方一定距離時(shí)控制水池清潔器12 的轉(zhuǎn)彎運(yùn)動(dòng)。在一些實(shí)施例中，控制系統(tǒng)10可以被配置成繪制放置水池清潔器12的環(huán)境 (即，游泳池，溫泉等），并利用同步定位和繪制（SLAM)技術(shù)，基于來自激光測距儀16和第 二傳感器組件24的輸出了解水池清潔器的周圍（即，無需先前的與環(huán)境相關(guān)的校準(zhǔn)或教 導(dǎo)）。通過這種方式，控制系統(tǒng)10能夠確定和優(yōu)化清潔路線并能夠操作水池清潔器12以遵 循（follow)這些優(yōu)化的清潔路線（例如，在特定時(shí)間段內(nèi)遍歷整個(gè)游泳池地板）。此外，控 制系統(tǒng)10能夠跟蹤清潔器的運(yùn)動(dòng)，以便跟蹤被清潔碎肩的路線，并確保已經(jīng)在特定時(shí)間段 之內(nèi)遍歷了整個(gè)游泳池地板。在一些實(shí)施例中，如下所述，可以由控制系統(tǒng)10使用基于特 征的擴(kuò)展卡爾曼濾波器（EKF) (Extended Kalman Filter)SLAM技術(shù)。在其他實(shí)施例中，可 以使用其他SLAM技術(shù)。 通常，為了使機(jī)器人運(yùn)載工具能夠自主在任何環(huán)境中執(zhí)行任務(wù)，它們必須能夠確定其 位置以及定位并且記住該環(huán)境中的障礙以及目標(biāo)對(duì)象的位置，或者換言之，它們必須能夠 SLAM?？梢允褂脭U(kuò)展的卡爾曼濾波器（EKF)來估計(jì)SLAM之后的情況。以下段落提供了根 據(jù)本發(fā)明一些實(shí)施例的EKF SLAM方法的概要。 從概率的意義上，SLAM的目標(biāo)是連同周圍環(huán)境的地圖估計(jì)水池清潔器12當(dāng)前姿態(tài)的 后續(xù)發(fā)展，其被表示為 p (xt, m| Zj：t, u !：t), Eq. 10 其中xt是水池清潔器12在時(shí)間t的姿態(tài)，m是地圖，z 1:t是測量結(jié)果，u 1:t為控制輸入。 EKF能夠假設(shè)狀態(tài)轉(zhuǎn)換和測量模型定義為 Xt= g(u t，Xt Ο + Πχ,?，t = 1，2· · ·， Eq. 11 Zt= h(x t) + qz，t， Eq. 12 其中8(.)和11(.)是非線性噪聲和加性噪聲，11)^和110是協(xié)方差分別為1^和^的 零平均值高斯過程。SLAM的EKF解落在稱為基于特征的方法的一類解中。在基于特征的 SLAM中，假設(shè)圍繞水池清潔器12的環(huán)境可以由稱為特征的一組不同的點(diǎn)表達(dá)。結(jié)果，完整 的SLAM狀態(tài)由清潔器12的狀態(tài)和地圖的狀態(tài)構(gòu)成。
其中X和y是清潔器12在二維（2D)平面中的位置，Θ是行進(jìn)方向。地圖由N個(gè)特征 來代表，并且每個(gè)特征在2D平面中的位置保持在狀態(tài)和中。 SLAM的EKF解能夠使用經(jīng)典的預(yù)測校正模型。更具體而言，EKF的預(yù)測步驟基于由以 上方程11給出的系統(tǒng)的狀態(tài)轉(zhuǎn)換模型，且可以定義為
其中xt i是根據(jù)前一時(shí)間步驟的狀態(tài)估計(jì)，X t :是當(dāng)前時(shí)間步驟的完整SLAM狀態(tài)的預(yù) 測，Stl是前一時(shí)間步驟的協(xié)方差估計(jì)，g是當(dāng)前時(shí)間步驟的協(xié)方差預(yù)測，且匕是8(.)相 對(duì)于在4和X t i評(píng)估的X t 4勺雅可比行列式。校正步驟來自由以上方程12給出的測量模 型，且可以定義為
其中氏是h(.)相對(duì)于在X t JP z t評(píng)估的X t i的雅可比行列式，z t是當(dāng)前時(shí)間的測量 結(jié)果。 一些實(shí)施例的當(dāng)前EKF SLAM技術(shù)能夠包括標(biāo)準(zhǔn)EKF中沒有出現(xiàn)的附加步驟，其與向 SLAM狀態(tài)增加新特征相關(guān)。例如，在遇到新特征時(shí)，必須將其集成到完整SLAM狀態(tài)（xt)以 及SLAM協(xié)方差（X t)兩者中?？梢杂上率蕉xSLAM狀態(tài)的增強(qiáng)（augmentation):
其中4是增加新特征之后的SLAM狀態(tài)，且f(.)基于當(dāng)前清潔器狀態(tài)和對(duì)特征的觀測 估計(jì)在全局系中新特征的位置。 相對(duì)于SLAM協(xié)方差的增強(qiáng)，對(duì)SLAM協(xié)方差的檢查表明其采取如下形式
[0070] 其中Σρ是清潔器估計(jì)的協(xié)方差，Σ ^是清潔器估計(jì)和地圖估計(jì)之間的協(xié)方 差，且Xt,m是地圖估計(jì)的協(xié)方差。根據(jù)Bailey等人（"Simultaneous localization and mapping (slam) ：Part ii'，。RoboticsMutomation Magazine，IEEE，13 (3)，pp. 108-117)， 可以將SLAM協(xié)方差的增強(qiáng)形式計(jì)算為
其中是增強(qiáng)的SLAM協(xié)方差，F(xiàn)tiX是f(.)相對(duì)于在x ,和z t評(píng)估的x t的雅可比行列 式，F(xiàn)t,^ f (·)相對(duì)于在X JP z t計(jì)算的z t的雅可比行列式。 參考本發(fā)明實(shí)施例的控制系統(tǒng)10,傳感器組件16、24能夠提供表示上述狀態(tài)轉(zhuǎn)換模型 輸入U(xiǎn)t和特征測量結(jié)果z t的數(shù)據(jù)。傳統(tǒng)上，對(duì)于地面運(yùn)載工具應(yīng)用，狀態(tài)轉(zhuǎn)換模型的輸入由 來自車輪編碼器的測距讀數(shù)構(gòu)成，而特征的位置是利用光探測和測距（LiDAR)來計(jì)算的。 不過，這些類型的傳感器不能在水下環(huán)境中工作。在典型的水下環(huán)境中，很多現(xiàn)有的傳感器 技術(shù)基于聲學(xué)，其中從多普勒速度記錄儀（DVL)向運(yùn)載工具提供測距數(shù)據(jù)，且利用SONAR傳 感器定位特征。不過，如上所述，由于在相對(duì)小的封閉環(huán)境（例如游泳池和溫泉）中可能產(chǎn) 生大量多次回波，所以基于聲學(xué)的傳感器是存在問題的。此外，當(dāng)前可用的傳感器存在傳感 器特有的問題。例如，如上所述，水池清潔器12能夠直接在水池地板上操作或非常接近水 池地板工作。在這樣的操作環(huán)境中，DVL傳感器的性能差，而且它們還尺寸大且價(jià)格高，使得 它們無法用于小型廉價(jià)水下載體中。此外，SONAR傳感器的問題是它們難以在實(shí)施基于特征 的SLAM方法時(shí)用于特征提取。更具體而言，SONAR只能報(bào)告位于SONAR傳感器掃描錐前方 一定距離處的對(duì)象的存在，使其難以識(shí)別可用于在基于特征的SLAM中生成地圖的獨(dú)特特 征。結(jié)果，在可以發(fā)生適當(dāng)數(shù)據(jù)關(guān)聯(lián)之前必須從多個(gè)位置觀測特征?；谟?jì)算機(jī)視覺算法和 上述傳感器組件16、24,本發(fā)明的控制系統(tǒng)10能夠克服以上問題并能夠確定狀態(tài)轉(zhuǎn)換模型 的控制輸入，以及封閉水下環(huán)境中的有效界標(biāo)測量結(jié)果（valid landmark measurement)， 如下文進(jìn)一步所述。 對(duì)于第二傳感器組件24而言，可以通過跟蹤相機(jī)獲取的連續(xù)圖像之間的一組點(diǎn)，從面 向下的相機(jī)計(jì)算視覺測距數(shù)據(jù)。根據(jù)幀之間點(diǎn)的平移和旋轉(zhuǎn)，可以確定清潔器取向的變化 (因此提供狀態(tài)轉(zhuǎn)換模型輸入）。例如，參考圖13,比較兩幅圖像以確定清潔器12的取向變 化（即，相對(duì)于全局參照系167所示的當(dāng)前圖像I。和之前圖像I P)。選擇要在幀間跟蹤的 點(diǎn)的一種方法是從圖像隨機(jī)選擇點(diǎn)（例如圖13中的點(diǎn)162)。不過，選擇的所得點(diǎn)可能難 以唯一地識(shí)別，從而跟蹤點(diǎn)變得相當(dāng)困難。為了緩解這個(gè)問題，根據(jù)一些實(shí)施例，可以使用 具有其內(nèi)置功能GoodFeaturesToTrack的開源圖像處理庫OpenCV。GoodFeaturesToTrack 功能選擇圖像中的角作為能夠容易識(shí)別和跟蹤的特征。在一個(gè)實(shí)施例中，可以基于Shi和 Tomasi 提出的方法計(jì)算角（"Good features to track'，，Computer Vision and Pattern Recognition，1994。 Proceedings CVPR'94,1994IEEE Computer Society Conference, pp. 593-600)，這種方法首先利用Sobel算子計(jì)算點(diǎn)周圍的Hessian矩陣，以計(jì)算二階導(dǎo)數(shù)。 然后比較Hessian矩陣的兩個(gè)本征值的最小值，如果其高于預(yù)設(shè)的最小閾值，選擇該點(diǎn)作 為有效角。利用選擇的一組可跟蹤的點(diǎn)，可以通過從I p到IJS蹤點(diǎn)，計(jì)算如圖13所示的幀 間位置的變化。 為了在幀之間跟蹤點(diǎn)，可以使用多步算法。首先，例如，可以使用內(nèi)核大?。╧ernel size)為7的拉普拉斯算符濾波器過濾IP。可以使用過濾的圖像而不是原始圖像進(jìn)行跟蹤， 以便考慮兩幀之間光照條件的變化（例如，以防止由于陰影或光亮的變化導(dǎo)致的跟蹤性能 的劣化）。 在過濾Ip之后，可以對(duì)圖像執(zhí)行GoodFeaturesToTrack功能，以計(jì)算在幀之間進(jìn)行跟 蹤的點(diǎn)集。然后可以使用對(duì)Ip使用的相同方法過濾Ic。然后可以利用交叉相關(guān)（cross correlation)技術(shù)，例如由 Nourani-vatani 等人描述的技術(shù)("Correlation Based Visual Odometry for Ground Vehicles"。Journal of Field Robotics，28(5)，ρρ· 742-768)，在 Ic中找到從IP選擇的每個(gè)點(diǎn)。例如，可以從IP選擇包含點(diǎn)的窗口，并可以在點(diǎn)窗口和I c之 間進(jìn)行交叉相關(guān)。交叉相關(guān)最大值的位置對(duì)應(yīng)于Ic中的點(diǎn)的位置?？梢岳眯〗嵌冉?和2D齊次變換方程的線性化版本來確定I#P I ε中的點(diǎn)之間的關(guān)系：
其中\(zhòng)丨，^和3^分別是該點(diǎn)在。和1。中的叉和7位置，3；!，5,50是清潔器在 相機(jī)參照系中的變化和取向的分量。整理方程22獲得 yp δ θ + δ χ = xc-xp, Eq. 23 -Xp δ θ + δ y = yc-yp, Eq. 24 可以針對(duì)所有被跟蹤的點(diǎn)將其組合為
其中i = l，2，……，M，M是被跟蹤點(diǎn)的數(shù)目?？梢酝ㄟ^利用SVD算法計(jì)算偽逆 (pseudoinverse)來發(fā)現(xiàn)所得的取向變化。然后可以利用事先從運(yùn)行校準(zhǔn)算法確定的校準(zhǔn) 常數(shù)將取向變化δ χ，δ jP δ e從像素單位變換到世界單位。 在建立狀態(tài)轉(zhuǎn)換模型中可以考慮兩個(gè)參照系：運(yùn)載工具參考系169,在其中測距數(shù)據(jù) 被收集；以及全局參考系167,清潔器12在其中工作，兩者都示于圖14中（其中全局參考 系167由yjP X g表示，而運(yùn)載工具參考系169由y'和X'表示）。視覺測距數(shù)據(jù)從相機(jī)參 考系到全局參考系的轉(zhuǎn)化（rotation)，從幾何的角度上來說，可以定義為：
其中Δχ和Ay是清潔器在全局系中的平移，Δχ'和Δ/是在運(yùn)載工具系中的平移。 所得的狀態(tài)轉(zhuǎn)換矩陣被定義為
其中