pping算法記錄的機器人狀態(tài)的校正。本發(fā)明中，Gmapping算法 和狀態(tài)估計算法在安裝有ROS系統(tǒng)的Linux系統(tǒng)下并發(fā)執(zhí)行，在機器人回到pu(x。，y。，0。） 后，Gmapping算法仍然使用粒子濾波器實現(xiàn)地圖信息的更新和機器人位姿的估計，按照原 算法繼續(xù)執(zhí)行，本發(fā)明采用的狀態(tài)估計算法則對Gmapping算法估計所得的機器人狀態(tài)進 行重新估計。
[0050] 記機器人的狀態(tài)方程和觀測方程分別為：
[005" Xk=Ik+A-i+Ik+iUk+Wk-i， （1)
[0052] Zk=Ik+A+Vk， （2)
[005引其中，Xk為機器人的狀態(tài)向量，Zk為觀測向量，Uk為系統(tǒng)輸入，W和V分別為機器 人的狀態(tài)和觀測噪聲（系統(tǒng)噪聲向量和觀測噪聲向量，向量中每個維度的噪聲一般可認為 服從高斯分布），可假設為高斯白噪聲，Iw為k+1維的單位矩陣。由于本發(fā)明中直接對狀 態(tài)向量，因此可W認為Xk和Zk存在等價關(guān)系。
[0054] 當機器人再次返回某個狀態(tài)Pi后（可假設其觀測到的該狀態(tài)為公,)，則基于卡爾 曼濾波器的狀態(tài)估計算法進行閉環(huán)校正，具體包括：
[005引設當前時刻表示為k，上一時刻表示為k-1。
[0056] ( -）通過狀態(tài)轉(zhuǎn)移矩陣計算上一時刻的狀態(tài)向量和輸入對當前時刻狀態(tài)的影 響，得到當前時刻的狀態(tài)預測值；W及通過狀態(tài)轉(zhuǎn)移矩和上一時刻的協(xié)方差矩陣值W及狀 態(tài)噪聲協(xié)方差矩陣得到協(xié)方差矩陣的預測值。
[0057] 具體分為兩步：
[0058] 1.計算狀態(tài)預測值
[0059]
[0060] 其中，i例/1-1)為k時刻的狀態(tài)預測值，支(A_l|A-：l)為k-1時刻的狀 態(tài)值，u(k-l)為k-1時刻的輸入，Iw為狀態(tài)轉(zhuǎn)移矩陣。本質(zhì)上是 1)，是矩陣的乘法，指k-1時刻狀態(tài)向k時刻的轉(zhuǎn)移，Iwu(k-l)同理。
[0061] 2.計算協(xié)方差矩陣的預測值
[0062]P化Ik-1)=IwP化-1Ik-1)Iw+Q似；
[0063] 其中，P化Ik-1)為協(xié)方差矩陣預測值；
[0064] 根據(jù)k-1時刻的值預測k時刻的值。IwP化-1Ik-1)Ik+康示的是IW.P化-1Ik-1). Iw,即矩陣的乘法。在狀態(tài)轉(zhuǎn)移的過程中要考慮誤差的影響，Q(k)是狀態(tài)噪聲協(xié)方差矩陣。
[0065](二）通過狀態(tài)方程和觀測方程實現(xiàn)觀測向量的更新，根據(jù)實際觀測值和當前時 刻的狀態(tài)預測值計算新息；所述狀態(tài)轉(zhuǎn)移矩陣當前時刻的狀態(tài)預測值計算新息；W及，通 過狀態(tài)轉(zhuǎn)移矩陣計算協(xié)方差矩陣預測值和觀測誤差的協(xié)方差矩陣，得到新息的協(xié)方差矩 陣。
[0066] 具體包括W下H步：
[0067] 1.通過狀態(tài)方程（1)和觀測方程（2)獲取Zk={...，p1，. 并將其重新設置為
[0068] 2.通過狀態(tài)預測計算新息
[0069]
[0070] 其中，i州*-1)是沒有噪聲影響時的狀態(tài)預測值，Z似是實際的觀測值，二者的 差就為新息，新息為新的有指導意義的信息，詳情可參考經(jīng)典卡爾曼濾波器。
[0071] 3.實現(xiàn)協(xié)方差矩陣的處理 [007引S似=IwP(k|k-l)Iw+R似；
[0073] 其中，S化）是新息的協(xié)方差矩陣；
[0074] 在公式（2)中，定義了觀測噪聲V，觀測噪聲為傳感器本身帶有一定的誤差引入 的，該個在任何領(lǐng)域均不可避免。R(k)就是觀測噪聲的協(xié)方差矩陣。
[007引IwP化Ik-1)Ik+沖的P化Ik-1)已在步驟2中計算獲得，通過矩陣乘法直接計算即 得到新息的協(xié)方差矩陣。
[0076] (H)通過狀態(tài)轉(zhuǎn)移矩陣計算當前時刻的協(xié)方差矩陣預測值，并計算新息的協(xié)方 差矩陣的逆矩陣，得到濾波器增益；通過上一時刻的狀態(tài)向量結(jié)合濾波器增益和新息計算 出當前時刻的狀態(tài)狀態(tài)向量；W及根據(jù)上一時刻的協(xié)方差矩陣值和增益計算出當前時刻的 協(xié)方差矩陣值。
[0077] 具體包括W下H步：
[0078] 1.計算卡爾曼濾波器增益
[0079]KGO=P化|k-l)IwS(k)-i;
[0080] 其中P化Ik-1)為當前時刻的協(xié)方差矩陣預測值；
[0081]S(k)是新息的協(xié)方差矩陣；
[008引首先計算P化Ik-1)Iw,等價于P化Ik-1).Iw(矩陣乘法）。然后計算S似的逆矩 陣S(krS最后得到濾波器增益K化)。
[0083] 2.實現(xiàn)狀態(tài)向量的更新
[0084]
[00財其中X化Ik)為k時刻的狀態(tài)向量；
[0086] i州fe-1)為k時亥I揃狀態(tài)預SJ值；
[0087] 通過矩陣乘法計算濾波器增益K(k)和新息y(A)，再與《州*-1),做加法計算得 到k時刻的狀態(tài)向量。
[0088] 3.實現(xiàn)協(xié)方差矩陣的更新
[008引P化Ik) = (I-KGOIw)P化Ik-I);
[0090] 其中P化Ik)為k時刻的協(xié)方差矩陣；
[0091] I是與K化)IW維數(shù)相同的單位矩陣；
[0092] 通過單位矩陣計算K(k)和P化Ik-I)即得到協(xié)方差矩陣的更新。
[0093] (四）生成新的地圖，具體包括W下一步：
[0094] 在得到k時刻的狀態(tài)向量x(k|k)和k時刻的協(xié)方差矩陣P(k|k)后算法就可W重 新迭代了。根據(jù)上述步驟更新所得的機器人狀態(tài)向量，重新記錄里程計和激光傳感器獲取 的環(huán)境信息，生成新的環(huán)境地圖，即為校正地圖。
[0095] 機器人再次返回{p。，. . .，pj后，在已標定過的點Pi,i= 1，. . .，k處，依然按照 Gmapping算法記錄里程計和激光傳感器的環(huán)境信息，實現(xiàn)地圖的進一步更新，得到更新的 校正地圖，圖5為本發(fā)明室內(nèi)環(huán)境地圖創(chuàng)建的閉環(huán)校正方法更新的校正地圖。
[0096] 機器人再次返回{p。，...，Pj后，分別執(zhí)行步驟4中基于卡爾曼濾波器的狀態(tài)估 計算法進行閉環(huán)校正，得到最終的校正地圖。圖6為本發(fā)明室內(nèi)環(huán)境地圖創(chuàng)建的閉環(huán)校正 方法最終的校正地圖。
[0097] 需要說明的是，機器人首次在環(huán)境中行走時，在滿足，
的條件 下，會記錄相應的里程計和激光傳感器的環(huán)境信息，該些記錄的點記為機器人的實時位姿 信息，記為{p。，...，PkK所W，當機器人再次進入環(huán)境中時，一般是在點{P。，...，Pj處或 其周圍的滿足一定誤差條件的區(qū)域內(nèi)（由于當時機器人是人為控制的，因此該個條件是能 夠滿足的）進行里程計和激光傳感器采集的環(huán)境信息的記錄。
[0098] 本發(fā)明提供了一種基于低成本、精度相對較低的激光傳感器的室內(nèi)環(huán)境地圖創(chuàng)建 的閉環(huán)校正方法。首先，通過對比分析不同精度及成本激光傳感器創(chuàng)建地圖的精度及閉環(huán) 效果，給出利用低成本、精度可接受的激光傳感器實現(xiàn)地圖創(chuàng)建的必要性及可行性；然后提 出一種基于卡爾曼濾波器的狀態(tài)估計算法，對利用低成本、低精度的激光傳感器數(shù)據(jù)創(chuàng)建 地圖過程中機器人的位姿進行校正，并基于校正后的機器人的位姿重新創(chuàng)建地圖，實現(xiàn)了 地圖的閉環(huán)，提高了地圖創(chuàng)建的精度。本發(fā)明提供了一種基于低成本、低精度激光傳感器的 地圖創(chuàng)建中的閉環(huán)校正方法，一方面，提高地圖創(chuàng)建的精度；另一方面，推進低成本激光傳 感器在日常生活及科學研究中的應用。
[0099] 顯然，本領(lǐng)域的技術(shù)人員可W對本發(fā)明進行各種改動和變形，而不脫離本發(fā)明的 精神和范圍。該樣，倘若本發(fā)明的該些修改屬于本發(fā)明權(quán)利要求及其同等技術(shù)的范圍之內(nèi)， 則本發(fā)明也意圖包含該些改動和變形在內(nèi)。
【主權(quán)項】
1. 一種室內(nèi)環(huán)境地圖創(chuàng)建的閉環(huán)校正方法，其特征在于，包括： 步驟1獲取環(huán)境信息，標記機器人的初始位姿信息； 步驟2機器人行走距離超過規(guī)定范圍后記錄機器人的實時位姿信息，采用Gmapping算 法根據(jù)所述初始位姿信息和所述實時位姿信息生成環(huán)境地圖； 步驟3檢測所述環(huán)境地圖是否出現(xiàn)閉環(huán)，若是，則繼續(xù)執(zhí)行以下步驟，若不是，則重復 執(zhí)行步驟2 ; 步驟4采用狀態(tài)估計算法重新估計機器人的所述實時位姿信息，并更新所述實時位姿 信息生成校正地圖。2. 如權(quán)利要求1所述的室內(nèi)環(huán)境地圖創(chuàng)建的閉環(huán)校正方法，其特征在于： 所述環(huán)境信息由里程計和激光傳感器獲取。3. 如權(quán)利要求2所述的室內(nèi)環(huán)境地圖創(chuàng)建的閉環(huán)校正方法，其特征在于： 所述激光傳感器為RPLidar激光雷達。4. 如權(quán)利要求1所述的室內(nèi)環(huán)境地圖創(chuàng)建的閉環(huán)校正方法，其特征在于： 所述步驟3檢測是否出現(xiàn)閉環(huán)通過場景標定實現(xiàn)。5. 如權(quán)利要求1所述的室內(nèi)環(huán)境地圖創(chuàng)建的閉環(huán)校正方法，其特征在于： 所述步驟2還包括記錄多個機器人的所述實時位姿信息生成機器人的狀態(tài)向量。6. 如權(quán)利要求5所述的室內(nèi)環(huán)境地圖創(chuàng)建的閉環(huán)校正方法，其特征在于： 所述步驟4采用狀態(tài)估計算法進行閉環(huán)校正，具體包括： 首先通過狀態(tài)轉(zhuǎn)移矩陣計算上一時刻的狀態(tài)向量和輸入對當前時刻狀態(tài)的影響，得到 當前時刻的狀態(tài)預測值；以及通過狀態(tài)轉(zhuǎn)移矩和上一時刻的協(xié)方差矩陣值以及狀態(tài)噪聲協(xié) 方差矩陣得到協(xié)方差矩陣的預測值； 再通過狀態(tài)方程和觀測方程實現(xiàn)觀測向量的更新，根據(jù)實際觀測值和當前時刻的狀態(tài) 預測值計算新息；所述狀態(tài)轉(zhuǎn)移矩陣當前時刻的狀態(tài)預測值計算新息；以及，通過狀態(tài)轉(zhuǎn) 移矩陣計算協(xié)方差矩陣預測值和觀測誤差的協(xié)方差矩陣，得到新息的協(xié)方差矩陣； 通過狀態(tài)轉(zhuǎn)移矩陣計算當前時刻的協(xié)方差矩陣預測值，并計算新息的協(xié)方差矩陣的逆 矩陣，得到濾波器增益；通過上一時刻的狀態(tài)向量結(jié)合濾波器增益和新息計算出當前時刻 的狀態(tài)狀態(tài)向量；以及根據(jù)上一時刻的協(xié)方差矩陣值和增益計算出當前時刻的協(xié)方差矩陣 值； 更新所述狀態(tài)向量和所述協(xié)方差矩陣； 根據(jù)更新所得的所述狀態(tài)向量生成所述校正地圖。7. 如權(quán)利要求1-6任意一項所述的室內(nèi)環(huán)境地圖創(chuàng)建的閉環(huán)校正方法，其特征在于： 所述Gmapping算法和所述狀態(tài)估計算法在安裝有ROS的Linux系統(tǒng)下并發(fā)執(zhí)行。
【專利摘要】本發(fā)明提供了一種室內(nèi)環(huán)境地圖創(chuàng)建的閉環(huán)校正方法，包括：步驟1獲取環(huán)境信息，標記機器人的初始位姿信息；步驟2機器人行走距離超過規(guī)定范圍后記錄機器人的實時位姿信息，采用Gmapping算法根據(jù)初始位姿信息和實時位姿信息生成環(huán)境地圖；步驟3檢測環(huán)境地圖是否出現(xiàn)閉環(huán)，若是，則繼續(xù)執(zhí)行以下步驟，若不是，則重復執(zhí)行步驟2；步驟4采用狀態(tài)估計算法重新估計機器人的實時位姿信息，并更新實時位姿信息生成校正地圖。本發(fā)明提供了一種基于低成本、低精度激光傳感器的地圖創(chuàng)建中的閉環(huán)校正方法，一方面，提高地圖創(chuàng)建的精度；另一方面，推進低成本激光傳感器在日常生活及科學研究中的應用。
【IPC分類】G01C21/00
【公開號】CN104964683
【申請?zhí)枴緾N201510313472
【發(fā)明人】張小*, 陳宗海, 王鵬, 章征貴
【申請人】上海物景智能科技有限公司
【公開日】2015年10月7日
【申請日】2015年6月4日