Θ^是來自指南針的測量結(jié)果。所得的控制輸入矢量
是 有噪聲的測量結(jié)果。為了擬合EKF所需要的形式，可以假設傳感器噪聲是協(xié)方差為Mt的零 平均值高斯過程。所得的系統(tǒng)狀態(tài)轉(zhuǎn)換模型可以被定義為
其具有協(xié)方差Rt= V tMtVtT，其中1為g (.)相對于在X t JP u t評估的u t的雅可比行列 式。于是，利用以上方法，可以使用來自第二傳感器組件24的測距數(shù)據(jù)來確定狀態(tài)轉(zhuǎn)換模 型輸入。 對于激光測距儀16,可以確定形狀信息，從而能夠進行特征檢測?？梢允褂盟_定的到 特征的距離和相對朝向確定EKF SLAM的測量模型（即，特征測量結(jié)果）。如圖15A-15B所 示，存在激光測距儀16在其中工作的兩個參考系：激光測距儀的局部參考系170和清潔器 12在其中操作的全局參考系172。在激光測距儀的局部參考系170中，可以將到特征的距 離和相對朝向定義為
Φ = atan2 (MX,L，My,L)， Eq. 33 其中Φ是到特征的相對朝向，r是距特征的距離，ΜχΛ和Μ?Λ是特征在局部系170中的 坐標。在全局系172中，r和Φ可以定義為
其中Mu和My是特征在全局系172中的位置。所得的測量模型為
其具有協(xié)方差為Qt的零平均值高斯加性噪聲，該高斯加性噪聲匹配EKF所需要的形 式。 如上所述，EKF SLAM是基于特征的技術(shù)，結(jié)果，特征檢測是實施本技術(shù)的關(guān)鍵方面?；?于一些實施例中使用的傳感器組件16,可以將環(huán)境中幾乎任何東西用作特征。例如，在室內(nèi) 環(huán)境中，常見特征可以包括墻壁和角，因為這些是容易識別的靜態(tài)對象。如上所述，可以從 激光測距儀16的距離測量結(jié)果提取諸如角的特征。例如，可以首先使用用于線識別的隨機 樣本一致（random sample consensus，RANSAC)算法的稍微改進版本。對RANSAC線識別的 改進涉及如何產(chǎn)生隨機樣本集合。例如，在標準的RANSAC算法中，樣本集合由并非已經(jīng)附 著到對象模型的隨機可能內(nèi)部點（inlier)構(gòu)成。這個可以被改進以減少對于并不是環(huán)境 中的真實墻壁的線的錯誤識別。更具體而言，為了克服這種錯誤識別的問題，可以通過如下 方式產(chǎn)生樣本集合：首先隨機選擇單個可能的內(nèi)部點，然后使用位于所選點周圍窗口之內(nèi) 的所有可能內(nèi)部點作為樣本集合。在線識別步驟之后，可以找到線之間的交點，并且如果那 些線之間的最小角度大于預定義閾值，可以將該交點表征為角。在圖16中示出了這種所得 角落識別的范例，包括激光測距儀距離測量結(jié)果174、兩條提取的線176、178和檢測到的角 180〇 與特征相關(guān)的EKF SLAM的另一分量稱為數(shù)據(jù)關(guān)聯(lián)，亦即，如果觀測到的特征已經(jīng)被 看到，將該特征與其自身相關(guān)聯(lián)，或者如果觀測到的特征從未被看到，增加該特征作為新特 征。在一些實施例中，可以使用門控搜索（gated search)算法。更具體而言，對于每次觀 測，基于清潔器狀態(tài)的當前估計，可以將預測的位置與當前跟蹤的特征的每個進行比較，并 且如果預測的位置落在當前跟蹤特征的門控距離之內(nèi)，可以將觀測與該特征相關(guān)聯(lián)，且如 果觀測不與任何被跟蹤特征相關(guān)聯(lián)，該觀測可以被假定為新特征并可以被增加到當前狀態(tài) 估計。在一些實施例中可以使用其他更復雜的方法。通過連續(xù)或周期性地更新清潔器的狀 態(tài)估計，且由于狀態(tài)估計還包含當前描述地圖的所有特征，所以也可以更新那些估計，這些 數(shù)據(jù)關(guān)聯(lián)方法能夠提供清潔器的真實位置的更好估計并減小誤差。 在一些實施例中，利用以上方法和技術(shù)，控制系統(tǒng)10能夠連續(xù)或周期性地測量水池清 潔器12前方的對象距離，繪制周圍環(huán)境，識別環(huán)境之內(nèi)的對象，定位水池清潔器在環(huán)境之 內(nèi)的位置，和/或在整個環(huán)境內(nèi)導航水池清潔器12。例如，基于繪圖和定位，控制系統(tǒng)10能 夠跟蹤和控制水池清潔器12的運動以優(yōu)化水池清潔器12在整個環(huán)境中的清潔路線。這可 以包括確定和存儲清潔路線，以及控制水池清潔器12以遵循清潔路線或跟蹤水池清潔器 12的運動路線，并周期性調(diào)節(jié)水池清潔器12的運動以確保在特定時間段內(nèi)遍歷環(huán)境的所 有區(qū)域。 本領(lǐng)域的技術(shù)人員將要認識到，盡管上文已經(jīng)結(jié)合特定實施例和范例描述了本發(fā)明， 但本發(fā)明未必受此限制，眾多其他實施例、范例、用途，以及實施例、范例和用途的修改和偏 離意在由本文所附權(quán)利要求涵蓋。所援引的每個專利和出版物的全部公開通過引用并入本 文，如同每個這樣的專利或出版物是通過引用單獨并入本文一樣。在如下權(quán)利要求中闡述 了本發(fā)明的各種特征和優(yōu)點。
【主權(quán)項】
1. 一種水池清潔器控制系統(tǒng)，用于檢測到所述水池清潔器前方對象的物理距離，所述 控制系統(tǒng)包括： 激光測距儀，包括第一激光線發(fā)生器、第二激光線發(fā)生器和相機，所述第一激光線發(fā)生 器和所述第二激光線發(fā)生器被定位以發(fā)射平行激光線，且所述相機被相對于所述第一激光 線發(fā)生器和所述第二激光線發(fā)生器定位以拍攝投射到所述對象上的激光線的圖像；以及 與所述激光測距儀通信的控制器，其被配置成 控制所述第一激光線發(fā)生器和所述第二激光線發(fā)生器的操作以發(fā)射激光線， 控制所述相機以拍攝投射到所述對象上的激光線的圖像， 從所述相機接收所述圖像， 計算所述圖像中所述激光線之間的像素距離，以及 基于所述像素距離計算所述相機和所述對象之間的物理距離。2. 根據(jù)權(quán)利要求1所述的控制系統(tǒng)，其中所述第一激光線發(fā)生器和所述第二激光線發(fā) 生器垂直安裝于彼此之上并分開一垂直距離。3. 根據(jù)權(quán)利要求2所述的控制系統(tǒng)，其中所述控制器被進一步配置成基于所述第一激 光線發(fā)生器和所述第二激光線發(fā)生器之間的所述垂直距離計算所述相機和所述對象之間 的所述物理距離。4. 根據(jù)權(quán)利要求1所述的控制系統(tǒng)，其中相對于所述相機定位所述第一激光線發(fā)生器 和所述第二激光線發(fā)生器以發(fā)射平行于所述相機視軸的激光線。5. 根據(jù)權(quán)利要求1所述的控制系統(tǒng)，其中所述第一激光線發(fā)生器和所述第二激光線發(fā) 生器是綠色激光線發(fā)生器。6. 根據(jù)權(quán)利要求1所述的控制系統(tǒng)，其中所述第一激光線發(fā)生器和所述第二激光線發(fā) 生器被相對于所述相機定位以發(fā)射平行于地表面的激光線。7. 根據(jù)權(quán)利要求1所述的控制系統(tǒng)，其中所述第一激光線發(fā)生器和所述第二激光線發(fā) 生器均以大約60度扇形角發(fā)射所述激光線。8. -種用于水下環(huán)境的自主機器人水池清潔器，所述水池清潔器包括： 測距儀，所述測距儀被配置成發(fā)射第一線和第二線，并檢測所述第一線和所述第二線 在所述環(huán)境的特征上的投影； 相機，配置成拍攝所述水池清潔器下方環(huán)境的地板表面的圖像； 控制器，與所述測距儀和所述相機通信，所述控制器被配置成： 基于所檢測到的所述第一線和所述第二線在所述特征上的投影來確定所述測距儀和 所述特征之間的物理距離， 基于拍攝的圖像確定視覺測距數(shù)據(jù)， 基于所述物理距離和所述視覺測距數(shù)據(jù)繪制所述環(huán)境，以及 跟蹤所述水池清潔器在所述環(huán)境之內(nèi)的位置。9. 根據(jù)權(quán)利要求8所述的水池清潔器，還包括與所述控制器通信的定向控制系統(tǒng)，所 述定向控制系統(tǒng)被配置成在所述地板表面上移動所述水池清潔器，所述控制器被配置成基 于所繪制的環(huán)境來操作所述定向控制系統(tǒng)以移動所述水池清潔器。10. 根據(jù)權(quán)利要求9所述的水池清潔器，其中所述控制器被配置成基于所繪制的環(huán)境 生成所述水池清潔器的清潔路線，并操作所述定向控制系統(tǒng)以沿所述清潔路線移動所述水 池清潔器。11. 根據(jù)權(quán)利要求8所述的水池清潔器，其中檢測所述第一線和所述第二線在特征上 的投影包括拍攝投射到所述特征上的所述第一線和所述第二線的圖像，所述控制器被配置 成通過將所述圖像中投射到所述特征上的所述第一線和所述第二線之間的像素距離關(guān)聯(lián) 到所述物理距離來確定所述測距儀和所述特征之間的物理距離。12. 根據(jù)權(quán)利要求8所述的水池清潔器，其中所述控制器被配置成利用基于特征的擴 展卡爾曼濾波器同步定位和繪制來繪制環(huán)境。13. -種在水下環(huán)境中利用雙平面激光測距儀確定到對象的距離的方法，所述方法包 括如下步驟： 向所述對象上投射第一線； 向所述對象上投射與所述第一線平行的第二線； 拍攝所述對象上投射的第一線和投射的第二線的圖像； 將所述圖像分割成獨立的圖像片段； 對于每個圖像片段： 提取所述圖像片段中的顏色平面以獲得灰度級圖像片段； 檢測所述灰度級圖像片段中的可能的線邊緣； 利用所述可能的線邊緣提取線段； 對所述線段分組以獲得所述灰度級圖像片段中的所述第一投影線和所述第二投影 線； 計算所述灰度級圖像片段中所述第一投影線和所述第二投影線之間的像素距離；以及 基于所述像素距離計算到所述對象的距離。
【專利摘要】本發(fā)明的實施例提供了一種水池清潔器控制系統(tǒng)，包括激光測距儀，激光測距儀具有第一激光線發(fā)生器、第二激光線發(fā)生器和相機。第一激光線發(fā)生器和第二激光線發(fā)生器被定位為發(fā)射平行的激光線，并且相機被定位為拍攝對象上投射的激光線的圖像?？刂葡到y(tǒng)還包括與激光測距儀通信的控制器，其被配置成控制激光線發(fā)生器的操作以發(fā)射激光線并控制相機以拍攝圖像?？刂破鬟€被配置成從相機接收圖像，計算圖像中激光線之間的像素距離，并基于像素距離計算相機和對象之間的物理距離。
【IPC分類】G01C3/32
【公開號】CN105308414
【申請?zhí)枴緾N201380041089
【發(fā)明人】A·萊奧內(nèi)薩, C·H.·卡因, B·J.·布思
【申請人】濱特爾水池水療公司, 弗吉尼亞技術(shù)知識資產(chǎn)公司
【公開日】2016年2月3日
【申請日】2013年6月27日
【公告號】CA2877919A1, EP2867611A2, US9086274, US20140009748, US20150267433, WO2014004929A2, WO2014004929A3, WO2014004929A9