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

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