專(zhuān)利名稱(chēng)：一種智能移動(dòng)機(jī)器人的定位與環(huán)境建模方法
技術(shù)領(lǐng)域：
本發(fā)明涉及自動(dòng)控制領(lǐng)域，特別涉及一種移動(dòng)機(jī)器人的定位與環(huán)境建模方法。
背景技術(shù)：
移動(dòng)機(jī)器人是傳感器技術(shù)、控制技術(shù)、信息處理技術(shù)、機(jī)械加工技術(shù)、電子技術(shù)、計(jì)算技術(shù)等多門(mén)技術(shù)相互結(jié)合而形成的。在這些技術(shù)的基礎(chǔ)上，移動(dòng)機(jī)器人主要研究以下內(nèi)容機(jī)器結(jié)構(gòu)、多機(jī)器人系統(tǒng)、體系結(jié)構(gòu)、路徑規(guī)劃、人工智能、導(dǎo)航與定位、多傳感器信息融合技術(shù)、人機(jī)交互、反饋鎮(zhèn)定以及跟蹤控制。在以上的研究?jī)?nèi)容中，有一項(xiàng)技術(shù)是移動(dòng)機(jī)器人的實(shí)際應(yīng)用中不可或缺的，這項(xiàng)技術(shù)就是導(dǎo)航與定位的研究。移動(dòng)機(jī)器人，顧名思義這種機(jī)器人不是固定在某一地方不動(dòng)，而是根據(jù)任務(wù)的要求隨時(shí)的采取相應(yīng)措施從起點(diǎn)到達(dá)目標(biāo)點(diǎn)。而在這個(gè)過(guò)程中，移動(dòng)機(jī)器人需要知道自己該往哪兒走，需要知道自己在哪兒，需要知道自己是不是到了目的地。而對(duì)這些問(wèn)題的回答，就形成了移動(dòng)機(jī)器人的導(dǎo)航與定位研 究?jī)?nèi)容。Leonard和Durrant-Whyte兩位研究人員對(duì)這個(gè)問(wèn)題做了一個(gè)具有綱領(lǐng)性的總結(jié)，也即移動(dòng)機(jī)器人在具備以下幾個(gè)條件的情況下能夠自主的進(jìn)行移動(dòng)，避免時(shí)刻受到人為因素的控制=(I)Where am I (我在哪里？)在移動(dòng)機(jī)器人的導(dǎo)航過(guò)程中，移動(dòng)機(jī)器人需要時(shí)刻的知道自己所處的位置，只有在知道了自己所處的位置，才能為下一步需要采取何種動(dòng)作提高判斷依據(jù)。這個(gè)問(wèn)題就是移動(dòng)機(jī)器人的定位問(wèn)題，在很好的回答了這個(gè)問(wèn)題的基礎(chǔ)上，才有可能解決后面兩個(gè)問(wèn)題；(2) Where am I going (我要去哪里？)在移動(dòng)機(jī)器人獲得任務(wù)后，它就得考慮在完成一步之后，下一步應(yīng)該往哪里走，或者是它的目的地在什么地方。這個(gè)問(wèn)題就需要移動(dòng)機(jī)器人對(duì)所處環(huán)境進(jìn)行建模，也就是環(huán)境建?；蛘哒f(shuō)是地圖構(gòu)建的研究；(3)How should I get there (我應(yīng)該如何到那兒？)一旦機(jī)器人在知曉了自己的當(dāng)前位置和下一步要去哪里之后，它就需要考慮自己在從當(dāng)前位置到達(dá)下一步位置應(yīng)該怎么走，也就是在這個(gè)過(guò)程中，需要走的路徑盡可能的短并且不能碰到障礙物。這就是移動(dòng)機(jī)器人的路徑規(guī)劃研究，是機(jī)器人的運(yùn)動(dòng)控制中的一部分研究?jī)?nèi)容。因此，對(duì)于移動(dòng)機(jī)器人的定位與環(huán)境建模進(jìn)行研究，可以擴(kuò)展移動(dòng)機(jī)器人的運(yùn)動(dòng)范圍、使用功能，能夠?qū)σ苿?dòng)機(jī)器人的信息無(wú)障礙研究和自主式移動(dòng)提供巨大的技術(shù)支撐。在移動(dòng)機(jī)器人的定位和環(huán)境建模研究方面，傳統(tǒng)上的方法都是對(duì)這兩個(gè)問(wèn)題進(jìn)行單獨(dú)的分析。因此，沒(méi)有考慮過(guò)將兩者進(jìn)行統(tǒng)一的研究。我們知道，在移動(dòng)機(jī)器人的定位中，如果在有精確的地圖的情況下，機(jī)器人的定位準(zhǔn)確性就很高。但是，由于移動(dòng)機(jī)器人本身的工作特性，我們不可能每次都能知道機(jī)器人工作環(huán)境的具體情況。在這種情況下，移動(dòng)機(jī)器人對(duì)環(huán)境的認(rèn)知就只有依靠自己攜帶的傳感器來(lái)采集環(huán)境信息。因此，當(dāng)機(jī)器人所處的環(huán)境比較陌生的情況下，機(jī)器人會(huì)面臨這一個(gè)兩難的問(wèn)題即為了構(gòu)建環(huán)境地圖，機(jī)器人需要清楚的知道自己在每個(gè)時(shí)刻所處的位置，為了精確的定位，移動(dòng)機(jī)器人需要有一個(gè)高準(zhǔn)確率的地圖。而這個(gè)問(wèn)題，可以用一個(gè)非常形象的比喻來(lái)描述，即“雞與蛋”的問(wèn)題。為了準(zhǔn)確并且高效的感知環(huán)境，移動(dòng)機(jī)器人需要將定位問(wèn)題與環(huán)境建模問(wèn)題同時(shí)考慮。在未知環(huán)境模型下移動(dòng)機(jī)器人的定位導(dǎo)航需要構(gòu)造環(huán)境的模型，這種在確定自身位置的同時(shí)并構(gòu)造環(huán)境模型的過(guò)程，被稱(chēng)為SLAM (Simultaneous Localizationand Mapping)。移動(dòng)機(jī)器人的同時(shí)定位與環(huán)境建模(SLAM)最早由Smith、Self和Cheeseman提出，由于其重要的理論與應(yīng)用價(jià)值，被諸多研究人員認(rèn)為是實(shí)現(xiàn)真正的全自主移動(dòng)機(jī)器人的關(guān)鍵。SLAM問(wèn)題歸納起來(lái)是一個(gè)“預(yù)測(cè)一觀測(cè)一更新”的過(guò)程，主要包括如下迭代步驟
(1)根據(jù)當(dāng)前的機(jī)器人位姿估計(jì)和地圖信息對(duì)下一步的機(jī)器人位姿和路標(biāo)位置進(jìn)行預(yù)測(cè)；
(2)在下一步中通過(guò)外部傳感器觀測(cè)環(huán)境中的路標(biāo)；(3)利用觀測(cè)結(jié)果校正第一步預(yù)測(cè)的定位和建模結(jié)果。上述步驟迭代進(jìn)行，在此過(guò)程中機(jī)器人對(duì)自身位姿和路標(biāo)位置的估計(jì)逐漸準(zhǔn)確。SLAM中使用得最廣泛的擴(kuò)展卡爾曼濾波算法(Extended Kalman Filter, EKF)在線性化過(guò)程中對(duì)非線性函數(shù)進(jìn)行Taylor級(jí)數(shù)展開(kāi)，忽略二次及其以上階次得到近似的線性模型，但是該算法在對(duì)于Taylor展開(kāi)式的高階次項(xiàng)的忽略，線性化所產(chǎn)生的誤差就會(huì)比較大。所以，此時(shí)應(yīng)用于SLAM的EKF就會(huì)增大誤差。需要對(duì)EKF進(jìn)行相應(yīng)的改進(jìn)。 因此急需一種適用于移動(dòng)機(jī)器人同時(shí)進(jìn)行定位與環(huán)境建模的高效率的計(jì)算方法。

發(fā)明內(nèi)容
有鑒于此，本發(fā)明所要解決的技術(shù)問(wèn)題是提供一種適用于移動(dòng)機(jī)器人同時(shí)進(jìn)行定位與環(huán)境建模的高效率的計(jì)算方法。該算法以常規(guī)的EKF算法為基礎(chǔ)，對(duì)EKF在數(shù)學(xué)上的缺陷進(jìn)行優(yōu)化、修改，使之能夠很適用于移動(dòng)機(jī)器人的同時(shí)定位與環(huán)境建模中，提高算法在SLAM中的估計(jì)效果。本發(fā)明的目的是這樣實(shí)現(xiàn)的本發(fā)明提供的一種智能移動(dòng)機(jī)器人的定位與環(huán)境建模方法，包括以下步驟SI :形成修正迭代擴(kuò)展卡爾曼濾波算法，并確定迭代次數(shù)；S2 :建立移動(dòng)機(jī)器人的運(yùn)動(dòng)模型Xk ；S3 :建立移動(dòng)機(jī)器人的觀測(cè)模型Zk ；S4 :初始化移動(dòng)機(jī)器人的先驗(yàn)狀態(tài)變量乓、先驗(yàn)協(xié)方差矩陣ZT、系統(tǒng)的過(guò)程噪聲協(xié)方差Qlri及觀測(cè)噪聲協(xié)方差Rlri ；S5 :計(jì)算移動(dòng)機(jī)器人運(yùn)動(dòng)模型的位置雅可比矩陣S6 :計(jì)算移動(dòng)機(jī)器人控制輸入雅可比矩陣▽/S7 :計(jì)算觀測(cè)模型的觀測(cè)雅可比矩陣Jh ；S8 :計(jì)算卡爾曼增益Kk ；S9 :更新后驗(yàn)狀態(tài)估計(jì)毛和后驗(yàn)狀態(tài)估計(jì)協(xié)方差矩陣Pk ；SlO :重復(fù)循環(huán)步驟S5至S9。進(jìn)一步，所述修正迭代擴(kuò)展卡爾曼濾波算法，具體如下時(shí)間更新(預(yù)測(cè))計(jì)算先驗(yàn)狀態(tài)變量 : = fh, ,計(jì)算先驗(yàn)狀態(tài)估計(jì)協(xié)方差矩陣..Pk = AkPk^Al + WkQkJVf，測(cè)量更新(校正)計(jì)算卡爾曼增益-.Kk= P-Jl [JhPhJji + VkRlJf )-'，由觀測(cè)變量Zk更新后驗(yàn)狀態(tài)估計(jì) 二 XiT + Kk{zk-Hk)，
后驗(yàn)狀態(tài)估計(jì)協(xié)方差矩陣更新
權(quán)利要求
1.一種智能移動(dòng)機(jī)器人的定位與環(huán)境建模方法，其特征在于它包括以下步驟 Si:形成修正迭代擴(kuò)展卡爾曼濾波算法，并確定迭代次數(shù)； 52:建立移動(dòng)機(jī)器人的運(yùn)動(dòng)模型Xk ； 53:建立移動(dòng)機(jī)器人的觀測(cè)模型Zk ； 54:初始化移動(dòng)機(jī)器人的先驗(yàn)狀態(tài)變量％、先驗(yàn)協(xié)方差矩陣iT、系統(tǒng)的過(guò)程噪聲協(xié)方差Qk-!及觀測(cè)噪聲協(xié)方差Rlri ； 55:計(jì)算移動(dòng)機(jī)器人運(yùn)動(dòng)模型的位置雅可比矩陣V/Xi 56:計(jì)算移動(dòng)機(jī)器人控制輸入雅可比矩陣V/ ; 57:計(jì)算觀測(cè)模型的觀測(cè)雅可比矩陣Jh ； S8:計(jì)算卡爾曼增益Kk; S9 :更新后驗(yàn)狀態(tài)估計(jì)毛和后驗(yàn)狀態(tài)估計(jì)協(xié)方差矩陣Pk ； SlO :重復(fù)循環(huán)步驟S5至S9。
2.根據(jù)權(quán)利要求I所述的智能移動(dòng)機(jī)器人的定位與環(huán)境建模方法，其特征在于形成修正迭代擴(kuò)展卡爾曼濾波算法，具體如下 時(shí)間更新-預(yù)測(cè) 計(jì)算先驗(yàn)狀態(tài)變量 =， 計(jì)算先驗(yàn)狀態(tài)估計(jì)協(xié)方差矩陣-A: =AA-—A+WkQk^wI， 測(cè)量更新-校正計(jì)算卡爾曼增益:Kk = P-J1h (JhPhJ7h + VkRkVk7 Y1, 由觀測(cè)變量Zk更新后驗(yàn)狀態(tài)估計(jì)■太—=xk+Kk(zk-Hk)， 后驗(yàn)狀態(tài)估計(jì)協(xié)方差矩陣更新-A =(J-KkJk)Pk， 其中，4表示Xk在k時(shí)刻的先驗(yàn)估計(jì),毛是Xk在k時(shí)刻的后驗(yàn)估計(jì)是系統(tǒng)從k-1時(shí)刻的狀態(tài)向量變化到k時(shí)刻的控制輸入，Zk是k時(shí)刻的觀測(cè)向量，f( )是系統(tǒng)的狀態(tài)轉(zhuǎn)移函數(shù)表示k時(shí)刻的協(xié)方差估計(jì)，Ak和Wk分別是f(_)對(duì)xk和對(duì)W的偏導(dǎo)雅可比矩陣，和灰/分別是它們的轉(zhuǎn)置矩陣，Qk-!表示過(guò)程激勵(lì)噪聲協(xié)方差，Kk表示卡爾曼增益，Jh表示狀態(tài)向量Xk對(duì)測(cè)量向量Zk的增益，Vk是f ( )對(duì)V的偏導(dǎo)雅可比矩陣，Rk表示k時(shí)刻的噪聲協(xié)方差，0—1表示矩陣的逆，Zk表示測(cè)量向量，Hk表示非線性函數(shù)h對(duì)X的偏導(dǎo)雅可比矩陣，I表示單位矩陣。
3.根據(jù)權(quán)利要求2所述的智能移動(dòng)機(jī)器人的定位與環(huán)境建模方法，其特征在于建立移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型具體如下
4.根據(jù)權(quán)利要求3所述的智能移動(dòng)機(jī)器人的定位與環(huán)境建模方法，其特征在于建立移動(dòng)機(jī)器人觀測(cè)模型具體如下 zk = h (xk, vk)
5.根據(jù)權(quán)利要求4所述的智能移動(dòng)機(jī)器人的定位與環(huán)境建模方法，其特征在于計(jì)算移動(dòng)機(jī)器人的位置雅可比矩陣如下
6.根據(jù)權(quán)利要求5所述的智能移動(dòng)機(jī)器人的定位與環(huán)境建模方法，其特征在于計(jì)算移動(dòng)機(jī)器人的控制輸入的雅可比矩陣7/ 如下
7.根據(jù)權(quán)利要求6所述的智能移動(dòng)機(jī)器人的定位與環(huán)境建模方法，其特征在于觀測(cè)模型實(shí)際觀測(cè)數(shù)據(jù)通過(guò)移動(dòng)機(jī)器人上的傳感器獲得，所述觀測(cè)模型在第i個(gè)坐標(biāo)下的雅可比矩陣Jh的計(jì)算具體如下
8.根據(jù)權(quán)利要求I所述的智能移動(dòng)機(jī)器人的定位與環(huán)境建模方法，其特征在于所述迭代處理次數(shù)根據(jù)優(yōu)化效果和計(jì)算速度確定。
全文摘要
本發(fā)明公開(kāi)了一種智能移動(dòng)機(jī)器人的定位與環(huán)境建模方法，首先形成修正迭代擴(kuò)展卡爾曼濾波算法并確定迭代次數(shù)，然后建立移動(dòng)機(jī)器人的運(yùn)動(dòng)模型和觀測(cè)模型，初始化移動(dòng)機(jī)器人的狀態(tài)，計(jì)算位置雅可比矩陣和控制輸入雅可比矩陣計(jì)算、觀測(cè)雅可比矩陣等；最后通過(guò)求解卡爾曼增益矩陣，更新?tīng)顟B(tài)估計(jì)方程和協(xié)方差矩陣，并重復(fù)以上部分步驟。本發(fā)明以在移動(dòng)機(jī)器人的同時(shí)定位與環(huán)境建模領(lǐng)域中使用最為廣泛的擴(kuò)展卡爾曼濾波算法為核心，通過(guò)對(duì)該算法進(jìn)行改進(jìn)，使得算法的性能得到極大提高，能夠更好的滿(mǎn)足在SLAM中的應(yīng)用。也為移動(dòng)機(jī)器人在未知環(huán)境中的自主導(dǎo)航、完成復(fù)雜智能任務(wù)提供強(qiáng)大的技術(shù)支撐。
文檔編號(hào)G01C21/00GK102706342SQ20121017787
公開(kāi)日2012年10月3日 申請(qǐng)日期2012年5月31日 優(yōu)先權(quán)日2012年5月31日
發(fā)明者唐賢倫, 張毅, 徐曉東, 李敏, 羅元, 胡章芳, 蔡軍, 謝穎 申請(qǐng)人:重慶郵電大學(xué)