一種智能移動機器人的定位與環境建模方法
2023-12-09 17:32:26 2
專利名稱：一種智能移動機器人的定位與環境建模方法
技術領域：
本發明涉及自動控制領域，特別涉及一種移動機器人的定位與環境建模方法。
背景技術：
移動機器人是傳感器技術、控制技術、信息處理技術、機械加工技術、電子技術、計算技術等多門技術相互結合而形成的。在這些技術的基礎上，移動機器人主要研究以下內容機器結構、多機器人系統、體系結構、路徑規劃、人工智慧、導航與定位、多傳感器信息融合技術、人機互動、反饋鎮定以及跟蹤控制。在以上的研究內容中，有一項技術是移動機器人的實際應用中不可或缺的，這項技術就是導航與定位的研究。移動機器人，顧名思義這種機器人不是固定在某一地方不動，而是根據任務的要求隨時的採取相應措施從起點到達目標點。而在這個過程中，移動機器人需要知道自己該往哪兒走，需要知道自己在哪兒，需要知道自己是不是到了目的地。而對這些問題的回答，就形成了移動機器人的導航與定位研 究內容。Leonard和Durrant-Whyte兩位研究人員對這個問題做了一個具有綱領性的總結，也即移動機器人在具備以下幾個條件的情況下能夠自主的進行移動，避免時刻受到人為因素的控制=(I)Where am I (我在哪裡？)在移動機器人的導航過程中，移動機器人需要時刻的知道自己所處的位置，只有在知道了自己所處的位置，才能為下一步需要採取何種動作提高判斷依據。這個問題就是移動機器人的定位問題，在很好的回答了這個問題的基礎上，才有可能解決後面兩個問題；(2) Where am I going (我要去哪裡？)在移動機器人獲得任務後，它就得考慮在完成一步之後，下一步應該往哪裡走，或者是它的目的地在什麼地方。這個問題就需要移動機器人對所處環境進行建模，也就是環境建模或者說是地圖構建的研究；(3)How should I get there (我應該如何到那兒？)一旦機器人在知曉了自己的當前位置和下一步要去哪裡之後，它就需要考慮自己在從當前位置到達下一步位置應該怎麼走，也就是在這個過程中，需要走的路徑儘可能的短並且不能碰到障礙物。這就是移動機器人的路徑規劃研究，是機器人的運動控制中的一部分研究內容。因此，對於移動機器人的定位與環境建模進行研究，可以擴展移動機器人的運動範圍、使用功能，能夠對移動機器人的信息無障礙研究和自主式移動提供巨大的技術支撐。在移動機器人的定位和環境建模研究方面，傳統上的方法都是對這兩個問題進行單獨的分析。因此，沒有考慮過將兩者進行統一的研究。我們知道，在移動機器人的定位中，如果在有精確的地圖的情況下，機器人的定位準確性就很高。但是，由於移動機器人本身的工作特性，我們不可能每次都能知道機器人工作環境的具體情況。在這種情況下，移動機器人對環境的認知就只有依靠自己攜帶的傳感器來採集環境信息。因此，當機器人所處的環境比較陌生的情況下，機器人會面臨這一個兩難的問題即為了構建環境地圖，機器人需要清楚的知道自己在每個時刻所處的位置，為了精確的定位，移動機器人需要有一個高準確率的地圖。而這個問題，可以用一個非常形象的比喻來描述，即「雞與蛋」的問題。為了準確並且高效的感知環境，移動機器人需要將定位問題與環境建模問題同時考慮。在未知環境模型下移動機器人的定位導航需要構造環境的模型，這種在確定自身位置的同時並構造環境模型的過程，被稱為SLAM (Simultaneous Localizationand Mapping)。移動機器人的同時定位與環境建模(SLAM)最早由Smith、Self和Cheeseman提出，由於其重要的理論與應用價值，被諸多研究人員認為是實現真正的全自主移動機器人的關鍵。SLAM問題歸納起來是一個「預測一觀測一更新」的過程，主要包括如下迭代步驟
(1)根據當前的機器人位姿估計和地圖信息對下一步的機器人位姿和路標位置進行預測；
(2)在下一步中通過外部傳感器觀測環境中的路標；(3)利用觀測結果校正第一步預測的定位和建模結果。上述步驟迭代進行，在此過程中機器人對自身位姿和路標位置的估計逐漸準確。SLAM中使用得最廣泛的擴展卡爾曼濾波算法(Extended Kalman Filter, EKF)在線性化過程中對非線性函數進行Taylor級數展開，忽略二次及其以上階次得到近似的線性模型，但是該算法在對於Taylor展開式的高階次項的忽略，線性化所產生的誤差就會比較大。所以，此時應用於SLAM的EKF就會增大誤差。需要對EKF進行相應的改進。 因此急需一種適用於移動機器人同時進行定位與環境建模的高效率的計算方法。
發明內容
有鑑於此，本發明所要解決的技術問題是提供一種適用於移動機器人同時進行定位與環境建模的高效率的計算方法。該算法以常規的EKF算法為基礎，對EKF在數學上的缺陷進行優化、修改，使之能夠很適用於移動機器人的同時定位與環境建模中，提高算法在SLAM中的估計效果。本發明的目的是這樣實現的本發明提供的一種智能移動機器人的定位與環境建模方法，包括以下步驟SI :形成修正迭代擴展卡爾曼濾波算法，並確定迭代次數；S2 :建立移動機器人的運動模型Xk ；S3 :建立移動機器人的觀測模型Zk ；S4 :初始化移動機器人的先驗狀態變量乓、先驗協方差矩陣ZT、系統的過程噪聲協方差Qlri及觀測噪聲協方差Rlri ；S5 :計算移動機器人運動模型的位置雅可比矩陣S6 :計算移動機器人控制輸入雅可比矩陣▽/S7 :計算觀測模型的觀測雅可比矩陣Jh ；S8 :計算卡爾曼增益Kk ；S9 :更新後驗狀態估計毛和後驗狀態估計協方差矩陣Pk ；SlO :重複循環步驟S5至S9。進一步，所述修正迭代擴展卡爾曼濾波算法，具體如下時間更新(預測)計算先驗狀態變量 : = fh, ,計算先驗狀態估計協方差矩陣..Pk = AkPk^Al + WkQkJVf，測量更新(校正)計算卡爾曼增益-.Kk= P-Jl [JhPhJji + VkRlJf )-'，由觀測變量Zk更新後驗狀態估計 二 XiT + Kk{zk-Hk)，
後驗狀態估計協方差矩陣更新
權利要求
1.一種智能移動機器人的定位與環境建模方法，其特徵在於它包括以下步驟 Si:形成修正迭代擴展卡爾曼濾波算法，並確定迭代次數； 52:建立移動機器人的運動模型Xk ； 53:建立移動機器人的觀測模型Zk ； 54:初始化移動機器人的先驗狀態變量％、先驗協方差矩陣iT、系統的過程噪聲協方差Qk-!及觀測噪聲協方差Rlri ； 55:計算移動機器人運動模型的位置雅可比矩陣V/Xi 56:計算移動機器人控制輸入雅可比矩陣V/ ; 57:計算觀測模型的觀測雅可比矩陣Jh ； S8:計算卡爾曼增益Kk; S9 :更新後驗狀態估計毛和後驗狀態估計協方差矩陣Pk ； SlO :重複循環步驟S5至S9。
2.根據權利要求I所述的智能移動機器人的定位與環境建模方法，其特徵在於形成修正迭代擴展卡爾曼濾波算法，具體如下 時間更新-預測 計算先驗狀態變量 =， 計算先驗狀態估計協方差矩陣-A: =AA-—A+WkQk^wI， 測量更新-校正計算卡爾曼增益:Kk = P-J1h (JhPhJ7h + VkRkVk7 Y1, 由觀測變量Zk更新後驗狀態估計■太—=xk+Kk(zk-Hk)， 後驗狀態估計協方差矩陣更新-A =(J-KkJk)Pk， 其中，4表示Xk在k時刻的先驗估計,毛是Xk在k時刻的後驗估計是系統從k-1時刻的狀態向量變化到k時刻的控制輸入，Zk是k時刻的觀測向量，f( )是系統的狀態轉移函數表示k時刻的協方差估計，Ak和Wk分別是f(_)對xk和對W的偏導雅可比矩陣，和灰/分別是它們的轉置矩陣，Qk-!表示過程激勵噪聲協方差，Kk表示卡爾曼增益，Jh表示狀態向量Xk對測量向量Zk的增益，Vk是f ( )對V的偏導雅可比矩陣，Rk表示k時刻的噪聲協方差，0—1表示矩陣的逆，Zk表示測量向量，Hk表示非線性函數h對X的偏導雅可比矩陣，I表示單位矩陣。
3.根據權利要求2所述的智能移動機器人的定位與環境建模方法，其特徵在於建立移動機器人的運動學模型具體如下
4.根據權利要求3所述的智能移動機器人的定位與環境建模方法，其特徵在於建立移動機器人觀測模型具體如下 zk = h (xk, vk)
5.根據權利要求4所述的智能移動機器人的定位與環境建模方法，其特徵在於計算移動機器人的位置雅可比矩陣如下
6.根據權利要求5所述的智能移動機器人的定位與環境建模方法，其特徵在於計算移動機器人的控制輸入的雅可比矩陣7/ 如下
7.根據權利要求6所述的智能移動機器人的定位與環境建模方法，其特徵在於觀測模型實際觀測數據通過移動機器人上的傳感器獲得，所述觀測模型在第i個坐標下的雅可比矩陣Jh的計算具體如下
8.根據權利要求I所述的智能移動機器人的定位與環境建模方法，其特徵在於所述迭代處理次數根據優化效果和計算速度確定。
全文摘要
本發明公開了一種智能移動機器人的定位與環境建模方法，首先形成修正迭代擴展卡爾曼濾波算法並確定迭代次數，然後建立移動機器人的運動模型和觀測模型，初始化移動機器人的狀態，計算位置雅可比矩陣和控制輸入雅可比矩陣計算、觀測雅可比矩陣等；最後通過求解卡爾曼增益矩陣，更新狀態估計方程和協方差矩陣，並重複以上部分步驟。本發明以在移動機器人的同時定位與環境建模領域中使用最為廣泛的擴展卡爾曼濾波算法為核心，通過對該算法進行改進，使得算法的性能得到極大提高，能夠更好的滿足在SLAM中的應用。也為移動機器人在未知環境中的自主導航、完成複雜智能任務提供強大的技術支撐。
文檔編號G01C21/00GK102706342SQ20121017787
公開日2012年10月3日 申請日期2012年5月31日 優先權日2012年5月31日
發明者唐賢倫, 張毅, 徐曉東, 李敏, 羅元, 胡章芳, 蔡軍, 謝穎 申請人:重慶郵電大學