【文章內(nèi)容簡(jiǎn)介】 的位置； θk表示 k 時(shí)刻機(jī)器人的朝向 ；Δ lk是 k 時(shí)刻移動(dòng)機(jī)器人的位移 ； k? 表示 k 時(shí)刻機(jī)器人的偏航角 。 由于目標(biāo)是靜態(tài)物體，所以在世界坐標(biāo)系中的位置可以表示為 11,i i i iG k G k G k G kx x y y???? iGkx 和 iGky （ i=1， ， n）是 第 i 個(gè)目標(biāo)在世界坐標(biāo) 系中的位置。機(jī)器人獲得的目標(biāo)位置（ iRkx ,iRky ）是指第 i 個(gè)目標(biāo)在機(jī)器人坐標(biāo)系中的位置，因此還需將其轉(zhuǎn)換成世界坐標(biāo)，可表示為： 擴(kuò)展卡爾曼濾波 (EKF) 卡爾曼濾波器假設(shè)系統(tǒng)是線性系統(tǒng)，但實(shí)際的機(jī)器人運(yùn)動(dòng)模型與觀測(cè)模型是非線性的 。 因此，通常采用擴(kuò)展卡爾曼濾波器，基于 EKF 的建圖與定位可以歸納為 1 個(gè) 循環(huán)迭代的估計(jì)－校正過(guò)程， EKF 算法在處理不確定信息方面有獨(dú)到之處，因此 EKF 成為應(yīng)用最廣泛的 SLAM 方法 。 根據(jù)以上的公式和假設(shè)，可以設(shè)計(jì)卡爾曼濾波器公式為： 11()()k k kk k kX F X vY H X w???? 其中， 1kv? 和 kw 為互不相關(guān)且均值為零的正態(tài)白噪聲序列，方差分別為 kQ 和kR 。 而 k 時(shí)刻的狀態(tài)向量和量測(cè)向量可表示為如下形式： 量測(cè)值 kx 是從狀態(tài)預(yù)測(cè)向量中計(jì)算得到的目標(biāo)位置，它是指目標(biāo)點(diǎn)在機(jī)器人坐標(biāo)系中的具體位置，可以表示為 ： 所以，系統(tǒng)狀態(tài)方程和量測(cè)方程為： 1 1 1 1 ,k k k k kk k k kX F X u vY H X w? ? ? ?? ? ??? 其中， 39。12,. nk k k kF H H HFHX X X X??? ? ? ?? ? ??? ?????? ? ? ??? 更新方程 采用 EKF 實(shí)現(xiàn)系統(tǒng)狀態(tài)的估計(jì)與更新，當(dāng)機(jī)器人在運(yùn)動(dòng)過(guò)程中發(fā)現(xiàn)新的特征 時(shí)，需要根據(jù)新特征的觀測(cè)矢量 z(k+1)和機(jī)器人的當(dāng)前狀態(tài)計(jì)算新特征標(biāo)志的初始狀態(tài)，并更新?tīng)顟B(tài)矢量和協(xié)方差矩陣 P。 其中， 在 SLAM 中，系統(tǒng)的狀態(tài)包括機(jī)器人和環(huán)境特征在機(jī)器人坐標(biāo)系中的位置的估計(jì)，而協(xié)方差矩陣 P 表示估計(jì)的誤差 。 擴(kuò)展卡爾曼濾波方法對(duì)信息的處理一般分為預(yù)測(cè)和更新，并且此方法對(duì)信息的估計(jì)是無(wú)偏估計(jì) 。 卡爾曼濾波預(yù)測(cè)方程為： 111 1 1 1,.k k k kk k kTk k k k kX F X uY H XP F P F Q?????? ? ? ?????? 更新方程為： 1( ) ,( ) ,( ) ,TTk k k k k k kk K k k kk k k kK P H H P H RX X K Y ZP I K H P? ? ?????? ? ??? 其中， Q 是狀態(tài)噪聲協(xié)方差， R 是測(cè)量噪聲協(xié)方差， P 是誤差協(xié)方差， K 是增益矩陣 。 預(yù)測(cè)方程用來(lái)預(yù)測(cè)當(dāng)前狀態(tài)，誤差協(xié)方差矩陣為下一時(shí)刻獲得先驗(yàn)估計(jì) 。 新方程將先驗(yàn)估計(jì)和測(cè)量值結(jié)合，得到一個(gè)較精確的后驗(yàn)估計(jì) 。 SIFT 算法 Lowe D G 在 2021 年總結(jié)了現(xiàn)有的基于不變量技術(shù)的特征檢測(cè)方法，并正式提出了一種基于尺度空間的、對(duì)圖像縮放、旋轉(zhuǎn)甚至仿射變換保持不便性的圖像局部特征描述算子 — SIFT 算子，即尺度不變特征變換 。 SIFT 算法首先在尺度空間進(jìn)行特征檢測(cè)，并確定關(guān)鍵點(diǎn)的位置和關(guān)鍵點(diǎn)所處的尺度，然后使用關(guān)鍵點(diǎn)領(lǐng)域梯度的主方向作為該點(diǎn)的方向特征，以實(shí)現(xiàn)算子對(duì)尺度和方向的無(wú)關(guān)性 . 梯度和方向的計(jì)算方程為： 上 式為 （ x， y） 處梯度的模值和方向公式，其中 L 所用的尺度為每個(gè)關(guān)鍵點(diǎn)各自所在的尺度 。 在實(shí)際計(jì)算時(shí)，可在以關(guān)鍵點(diǎn)為中心的鄰域窗口內(nèi)采樣，并用直方圖統(tǒng)計(jì)鄰域像素的梯度方向 。 直方圖的峰值代表了該關(guān)鍵點(diǎn)處鄰域梯度的主方向， 即作為 該關(guān)鍵點(diǎn) 的方向 。 三、 移動(dòng)機(jī)器人雙目視覺(jué)系統(tǒng) 雙目系統(tǒng)簡(jiǎn)介 雙目視覺(jué)測(cè)距法是仿照人類(lèi)利用雙目感知距離的一種測(cè)距方法。人的雙眼從稍有不同的兩個(gè) 角 度去觀察客觀三維世界的景物 ， 由于幾何光學(xué)的投影 ， 離觀察者不同距離的物點(diǎn)在左、右兩眼現(xiàn)網(wǎng)膜上的像不是在相同的位置上 ， 這種在兩眼視網(wǎng)膜上的位置差就稱(chēng)之為雙眼視差 ,它反映了客觀景物的深度 （ 或距離 ）， 如圖 5 所示。首先運(yùn)用完全相同的兩個(gè)或多個(gè)攝像機(jī)對(duì)同一景物 從 不同位置成像獲得立體像對(duì) ， 通過(guò)各種算 法 匹配出相應(yīng)像點(diǎn) ， 從而計(jì)算出視差 ， 然后采用基于三角測(cè)量的方法恢復(fù)距離。立體視覺(jué)測(cè)距的難點(diǎn)是如何選擇合理的匹配特征和匹配準(zhǔn)則 ， 以保證 匹配的準(zhǔn)確性 。 圖 5 雙目立體視覺(jué)示意圖 設(shè)兩個(gè)圖像平面位于 一 個(gè)平面上 ， 兩個(gè) 攝像機(jī) 的坐標(biāo)軸相互平行 ， 且其 x 軸重 合 ； 攝像機(jī)之 間在 x 軸方向上的間距為基線距離 2a， 焦距均為 f。在這個(gè)模型中，場(chǎng)景中同 一個(gè)特征點(diǎn)在兩個(gè)攝像機(jī)圖像平面上的成像位置是不同的。我們將場(chǎng)景中同一點(diǎn)在兩個(gè)不同圖像中的投影點(diǎn)稱(chēng)為共軛對(duì)， 其中的一個(gè)投影點(diǎn)是另一個(gè)投影 （ correspondence）。求共軛對(duì) ， 就是求解對(duì)應(yīng)性問(wèn)題。兩幅圖像 重 疊時(shí)的共軛對(duì)點(diǎn)的位置之差（共軛對(duì)點(diǎn)之間的距蘺）稱(chēng)為視差（ disparity），通過(guò)兩個(gè)攝像機(jī)中心并且通過(guò)場(chǎng)景特征點(diǎn)的平面稱(chēng)為外極（ epipolar）平面，外極平面與圖像平面的交線為外極線。 假設(shè)坐標(biāo)系原點(diǎn)與左透鏡中心重合，場(chǎng)景點(diǎn) P（ X,Y,Z）在左、右圖像平面中的投影點(diǎn)分別為 PL、 PR，比較Δ OLPL和 Δ OMP相似，Δ ORRPR和Δ ORNP相似可得 2rlZaf xx? ? 因此，各種場(chǎng)景點(diǎn)的深度恢復(fù)可以通過(guò)計(jì)算視差來(lái)實(shí)現(xiàn)。注意 :由于數(shù)字圖像的離散特性 ， 視差值是一個(gè)整數(shù)。在實(shí)際中 ， 可以使用一些特殊算法使視差計(jì)算精度達(dá)到子像素 級(jí)。 因此 ， 對(duì)于一組給定的攝像機(jī)參數(shù) ， 提高場(chǎng)景點(diǎn)深度計(jì)算精度的有效途徑是增長(zhǎng)基線 距 離 2a， 即增大場(chǎng)景點(diǎn)對(duì)應(yīng)的視差。然而這種大角度立體方法也帶來(lái)了一些問(wèn)題 ， 主要的 問(wèn) 題有 ： (1)隨著基線距離的增加 ， 兩個(gè) 攝 像機(jī)的共同的可視范圍減小 ； (2)場(chǎng)景點(diǎn)對(duì)應(yīng)的視差值增大 ， 則搜索對(duì)應(yīng)點(diǎn)的范圍增大 ， 出現(xiàn)多義性的機(jī)會(huì)就增大 ； (3)由于透視投影引起的變形導(dǎo)致兩個(gè)攝像機(jī)獲取的兩幅圖像不完全相同 ，這就給確 定共軛對(duì)帶來(lái)了困難。 在圖 5 中，圖像中的每個(gè)特征點(diǎn)都位于第二幅圖像中的同一行。實(shí)際上，兩條外極線不一定完全在一條直線上，即垂直視差不為零。但為了簡(jiǎn)單起見(jiàn)，雙目立體算法中的許多算法都假設(shè)垂直視差為零。在實(shí)際應(yīng)用中經(jīng)常遇到的情況是兩個(gè)攝像機(jī)的光軸不平行，調(diào)整它們平行重合的技術(shù)即是攝像機(jī)的標(biāo)定。 移動(dòng)機(jī)器人雙目視覺(jué)系統(tǒng) 應(yīng)用 基于雙目視覺(jué)的移動(dòng)機(jī)器人 視覺(jué)導(dǎo)航 研究 針對(duì)立體視覺(jué)算法復(fù)雜 、 計(jì)算耗時(shí)等特點(diǎn)，提出了一種實(shí)時(shí)立體視覺(jué)系統(tǒng)的嵌入式實(shí)現(xiàn)方案，采用高性能多媒體 DSP 芯片 TMS320DM642 為核心構(gòu)建雙通道視頻采集系統(tǒng) 。 利用高性能 DSP 芯片，滿(mǎn)足了實(shí)時(shí)性要求，擺脫了以往用 PC 機(jī)實(shí)現(xiàn)的環(huán)境約束 ， 計(jì)算速度慢和大功耗等缺點(diǎn) 。 移動(dòng)機(jī)器人障礙物檢測(cè)原理 采用立體視覺(jué)的方法檢測(cè)障礙物，對(duì)需要判定的每一對(duì)左右圖像像素點(diǎn)進(jìn)行立體視覺(jué)的計(jì)算，得到其高度和深度信息 高度值高于地面一定閾值的點(diǎn)被認(rèn)為是障礙點(diǎn)，深度值小于一定閾值則需要改變行進(jìn)方向 其中移動(dòng)機(jī)器人，如圖 6所示 雙目 CCD 攝像機(jī)與 DM642 開(kāi)發(fā)板的視頻口相連接，構(gòu)成移動(dòng)機(jī)器人的立體視覺(jué)系統(tǒng) 移動(dòng)機(jī)器人的結(jié)構(gòu)示意圖，如圖 7 所示 其中， h=230mm 因此，通過(guò)計(jì)算三維 坐標(biāo) y 值與 h 值差值的絕對(duì)值，就可以判定空間某點(diǎn)是否為可行點(diǎn) 因此，取差值大于 5mm 則為不可行點(diǎn) 。 圖 6 移動(dòng)機(jī)器人 圖 7 結(jié)構(gòu)示意圖 障礙物目標(biāo)區(qū)域的提取 (1)為了降低原圖像 I中的噪聲，將圖像 I經(jīng)過(guò)高斯平滑濾波后得到圖像 M； (2)利用最大類(lèi)間方差分割算法，對(duì)圖像 M 求取最佳閾值 Th，進(jìn)行圖像初步二值化，灰度值大于 Th 為背景，灰度值小于 Th 為障礙物目標(biāo)，得到二值圖像 N； (3)步驟 2 所獲得的二值圖像 N 必然有著許多不規(guī)則的細(xì)節(jié)，如室內(nèi)環(huán)境地板的矩形拼塊縫隙，邊緣毛刺和內(nèi)部孤立的 “ 小島 ”等。 為此，采用一個(gè)較小的結(jié)構(gòu)元素對(duì)圖像 進(jìn)行 腐蝕運(yùn)算，得到二值圖像 ； (4)經(jīng)過(guò)腐蝕之后圖像的分割區(qū)域變小了，同時(shí)會(huì)使內(nèi)部的小孔面積擴(kuò)大，這就有可能丟掉障礙物輪廓邊緣和角點(diǎn)特征信息，導(dǎo)致后期圖像處理的信息丟失 為此，引入了結(jié)構(gòu)元素比較大的膨脹運(yùn)算，消除了障礙物區(qū)域中的小孔，恢復(fù)由于腐蝕運(yùn)算而丟失的輪廓邊緣，保證了分割區(qū)域的有效性 為此，選擇一個(gè)較大的結(jié)構(gòu)元素對(duì)圖像 進(jìn)行膨脹運(yùn)算，得到二值圖像 ；經(jīng)過(guò)上述四個(gè)步驟之后，最終得到障礙物目標(biāo)的大致區(qū)域，即二值圖像中的有效區(qū)域 。 移動(dòng)機(jī)器人的立體匹配策略 考慮到移動(dòng)機(jī)器人視覺(jué)障礙物檢測(cè)系統(tǒng)對(duì)算法的實(shí)時(shí)性要求以及硬件處理能力的限制，障礙物檢測(cè)系統(tǒng)采用一種基于圖像分割和 Harris 角點(diǎn)特征的區(qū)域匹配策略 其實(shí)現(xiàn)過(guò)程如下： 步驟 1：對(duì)采集到的原圖像進(jìn)行高斯平滑預(yù)處理； 步驟 2：應(yīng)用圖像分割方法，提取左右圖像障礙物目標(biāo)大致區(qū)域； 步驟 3：在提取出的障礙物目標(biāo)區(qū)域中，采用 Harris 角點(diǎn)探測(cè)器提取左右圖像的角點(diǎn)特征； 步驟 4：應(yīng)用互相關(guān)函數(shù) ZNCC（ Zero mean Normalized CrossCorrelation）算法對(duì)左右圖像角 點(diǎn)特征進(jìn)行相似性測(cè)度，得到初始匹配集 。 并對(duì)初始匹配集進(jìn)行極線約束和左右一致性校驗(yàn)，得到最終匹配結(jié)果 。 在立體匹配的搜索過(guò)程中，通常以互相關(guān)函數(shù)作為兩個(gè)搜索鄰域間的相似性