同步定位與地圖構建方法和設備與流程

本發明涉及機器人技術領域，特別涉及同步定位與地圖構建方法和設備。

背景技術：

同步定位與地圖構建(simultaneouslocalizationandmapping,簡稱slam)，其含義是當機器人處于一個事先未知的地方，通過在環境中不斷移動來增量式地構建周圍的環境地圖，并同步地計算自身在該環境中的位姿。由于機器人的位置在每一步運行中都存在誤差，而所觀測的環境特征信息與其高度相關，從根本上來說，slam是一個估計問題，即在有噪聲干擾的情況下，運用機器人自身攜帶的傳感器所觀察到的信息，來估計環境地圖和在此環境地圖中機器人的移動軌跡。目前，主要有基于電磁線、紅外射線、激光、視覺傳感器對機器人進行定位的技術。

但是，現有的技術，通常需要預先在環境當中設置參照物，因此存在便捷性不高以及精度不高的缺點。

技術實現要素：

本發明的主要目的是提供同步定位與地圖構建方法和設備，旨在提高同步定位與地圖構建便捷性和精度。

為實現上述目的，本發明提出的一種同步定位與地圖構建方法，用于多機器人，所述同步定位與地圖構建方法包括步驟：

加載預設的根據機器人的結構而建立機器人運動學模型；

通過傳感器掃描環境而在預設坐標系中建立參考環境地圖；

在移動至下一位置時，通過慣性導航系統預估所述機器人的第一估計位姿；并且，

通過傳感器掃描獲得更新環境地圖，根據所述更新環境地圖、參考環境地圖以及第一估計位姿獲得第二估計位姿以及新參考環境地圖；

根據所述第一估計位姿與第二估計位姿獲得更新位姿；

根據所述更新位姿對所述機器人的位姿進行更新。

優選的，所述通過傳感器掃描環境而在預設坐標系中建立參考環境地圖包括：

通過傳感器掃描采集現場環境的障礙物信息的二維點集；

將所述二維點集轉換為柵格地圖而在預設坐標系中建立參考環境地圖。

優選的，所述在移動至下一位置時，通過慣性導航系統預估所述機器人的第一估計位姿包括：

在移動至下一位置時，根據編碼器判別機器人運動距離；

根據電子羅盤與陀螺儀判別機器人運動方向；

根據所述機器人運動距離和運動方向，計算獲得所述機器人當前位姿，并且將所述機器人當前位姿作為所述機器人的第一估計位姿。

優選的，所述通過傳感器掃描獲得更新環境地圖，根據所述更新環境地圖、參考環境地圖以及第一估計位姿獲得第二估計位姿以及新參考環境地圖包括：

通過傳感器掃描獲得更新環境地圖；

根據所述第一估計位姿，采用迭代最近點算法將所述更新環境地圖映射到所述參考環境地圖上，并且獲得新參考環境地圖；

根據所述參考環境地圖、映射至所述參考環境地圖上的更新環境地圖，以及機器人學空間坐標轉換算法，獲得所述第二估計位姿。

優選的，所述同步定位與地圖構建方法還包括：

通過攝像設備獲得參考路標信息；

在移動至下一位置時，通過攝像設備獲得更新路標信息；

根據所述參考路標信息以及更新路標信息計算獲得卡爾曼增益矩陣，并且根據所述卡爾曼增益矩陣對所述機器人的位姿進行更新。

本發明還提供了一種同步定位與地圖構建設備，所述同步定位與地圖構建設備包括存儲器、處理器及存儲在所述存儲器上并可在所述處理器上運行的同步定位與地圖構建程序，所述同步定位與地圖構建程序被所述處理器執行時實現以下步驟：

加載預設的根據機器人的結構而建立機器人運動學模型；

通過傳感器掃描環境而在預設坐標系中建立參考環境地圖；

在移動至下一位置時，通過慣性導航系統預估所述機器人的第一估計位姿；并且，

通過傳感器掃描獲得更新環境地圖，根據所述更新環境地圖、參考環境地圖以及第一估計位姿獲得第二估計位姿以及新參考環境地圖；

根據所述第一估計位姿與第二估計位姿獲得更新位姿；

根據所述更新位姿對所述機器人的位姿進行更新。

優選的，所述處理器執行所述通過傳感器掃描環境而在預設坐標系中建立參考環境地圖包括：

通過傳感器掃描采集現場環境的障礙物信息的二維點集；

將所述二維點集轉換為柵格地圖而在預設坐標系中建立參考環境地圖。

優選的，所述處理器執行所述在移動至下一位置時，通過慣性導航系統預估所述機器人的第一估計位姿包括：

在移動至下一位置時，根據編碼器判別機器人運動距離；

根據電子羅盤與陀螺儀判別機器人運動方向；

根據所述機器人運動距離和運動方向，計算獲得所述機器人當前位姿，并且將所述機器人當前位姿作為所述機器人的第一估計位姿。

優選的，所述處理器執行所述通過傳感器掃描獲得更新環境地圖，根據所述更新環境地圖、參考環境地圖以及第一估計位姿獲得第二估計位姿以及新參考環境地圖包括：

通過傳感器掃描獲得更新環境地圖；

根據所述第一估計位姿，采用迭代最近點算法將所述更新環境地圖映射到所述參考環境地圖上，并且獲得新參考環境地圖；

根據所述參考環境地圖、映射至所述參考環境地圖上的更新環境地圖，以及機器人學空間坐標轉換算法，獲得第二估計位姿。

優選的，在所述根據所述更新位姿對所述機器人的位姿進行更新的步驟之后，所述處理器還用于執行所述同步定位與地圖構建程序，以實現以下步驟：

通過攝像設備獲得參考路標信息；

在移動至下一位置時，通過攝像設備獲得更新路標信息；

根據所述參考路標信息以及更新路標信息計算獲得卡爾曼增益矩陣，并且根據所述卡爾曼增益矩陣對所述機器人的位姿進行更新。

本發明所提供的同步定位與地圖構建方法，通過采用慣性導航系統則可以獲得第一估計位姿，進一步采用掃描障礙物的傳感器，則可以不斷的獲得環境地圖；再通過第一估計位姿指導前后環境地圖建立匹配關系，從而可以快速和精確地構建當前環境的地圖；進一步的通過匹配后的前后環境地圖而獲得第二估計位姿，再通過第一估計位姿和第二估計位姿相結合，而獲得更精確的更新位姿，從而使得機器人的定位精度更高，則降低了定位不準而帶來的累計誤差，進而又可以同步構建精度更高當前環境地圖。

附圖說明

為了更清楚地說明本發明實施例或現有技術中的技術方案，下面將對實施例或現有技術描述中所需要使用的附圖作簡單地介紹，顯而易見地，下面描述中的附圖僅僅是本發明的一些實施例，對于本領域普通技術人員來講，在不付出創造性勞動的前提下，還可以根據這些附圖示出的結構獲得其他的附圖。

圖1為本發明同步定位與地圖構建方法第一實施例的流程圖；

圖2為圖1中步驟s102的流程圖；

圖3為圖1中步驟s103的流程圖；

圖4為圖1中步驟s104的流程圖；

圖5為本發明同步定位與地圖構建方法第二實施例的流程圖；

圖6為本發明同步定位與地圖構建設備一實施例的示意圖；

圖7為圖6所示同步定位與地圖構建設備的工作流程示意圖。

本發明目的的實現、功能特點及優點將結合實施例，參照附圖做進一步說明。

具體實施方式

應當理解，此處所描述的具體實施例僅僅用以解釋本發明，并不用于限定本發明。

請參看圖1，本發明同步定位與地圖構建方法第一實施例，所述同步定位與地圖構建方法，用于多機器人，所述同步定位與地圖構建方法包括步驟：

步驟s101，加載預設的根據機器人的結構而建立機器人運動學模型。

由于機器人的結構不同，存在大小功能運動結構都不相同的情況，因此為了針對不同的機器人，可以預設針對各自結構的運動學模型，從而便于精確的描述機器人的運動位姿。具體的，首先運動學模型可以包括：

1、確定機器人自由度，例如為六自由度機器人，即由當前參考位置的三維坐標以及繞三個固定軸的角度，六個變量可以確定機器人的位姿。2、根據機器人底盤結構，確定機器人本體運動線速度與角速度之間的關系。3、利用模型對機器人移動位姿規律進行分析，并作為機器人定位的數學基礎。

步驟s102，通過傳感器掃描環境而在預設坐標系中建立參考環境地圖。

傳感器的種類可以是多種，例如激光傳感器、聲波傳感器以及電磁波傳感器(雷達)等等。這些都可以通過主動發射并且接受反射波來確定環境中障礙物。

步驟s103，在移動至下一位置時，通過慣性導航系統預估所述機器人的第一估計位姿。

所述下一位置，可以根據預設參數限定，例如車輪轉動了1圈、移動了1米或者是移動了1秒鐘時等等。所述慣性導航系統以牛頓力學定律為基礎，通過測量載體在慣性參考系的加速度，將它對時間進行積分，且把它變換到導航坐標系中，就能夠得到在導航坐標系中的速度、偏航角和位置等信息。本實施例可以采用例如捷聯式慣性導航系統、解析式慣性導航系統和半解析式慣性導航系等等。

步驟s104，通過傳感器掃描獲得更新環境地圖，根據所述更新環境地圖、參考環境地圖以及第一估計位姿獲得第二估計位姿以及新參考環境地圖。

所述更新環境地圖用于展示機器人的當前位置的環境狀態。可以通過對比更新環境地圖與參考環境地圖的變化來推算第二估計位姿。

其中，僅僅通過兩個地圖的匹配則效率較低，并且準確度較低。優選的，本實施例利用第一估計位姿來確定更新環境地圖相對參考環境地圖的大致變化方向，然后再根據該大致變化方向，對更新環境地圖與參考環境地圖進行相互匹配，則可以較快速且準確地確定更新環境地圖與參考環境地圖的關系。然后將更新環境地圖和參考環境地圖進行組合，從而獲得新參考環境地圖，達到實時描繪地圖的功能。

進一步的，還可以通過第二估計位姿和第一估計位姿進行比對計算，從而矯正第二估計位姿，再根據矯正的第二估計位姿而獲得更精確的新參考環境地圖。

當然，還可以通過利用第一估計位姿來獲得參考環境地圖的當前估計變化地圖，然后將估計變化地圖與更新環境地圖進行比對，從而確定參考環境地圖與更新環境地圖的確切匹配關系。例如：

參考環境地圖中具有五棵樹abcde，機器人向前移動。在下一位置時，通過第一估計位姿來確認該五棵樹abcde和機器人的相對位置關系，從而獲得當前估計變化地圖。在該當前估計變化地圖中，五棵樹abcde向后移動。同時，機器人通過掃描獲得更新環境地圖，在而更新環境地圖中也有五棵樹cdefg。然后通過估計變化地圖的樹cde的坐標信息，確認參考環境地圖和更新環境地圖中樹cde的匹配關系，然后根據該匹配結果將更新環境地圖映射到參考環境地圖中，從而獲得新的參考環境地圖；以及根據匹配關系確定時，通過計算參考環境地圖和更新環境地圖而獲得的第二估計位姿。

以上具體舉例僅僅作為本步驟的舉例說明，并不限定本步驟僅僅采用上述方案。

步驟s105，根據所述第一估計位姿與第二估計位姿獲得更新位姿。優選的，可以根據所述第一估計位姿與第二估計位姿，以及擴展卡爾曼濾波算法獲得更新位姿。其中，運用擴展卡爾曼濾波算法在后文中陳述。

由于第一估計位姿和第二估計位姿都屬于估計值，而哪一個估計值更精確則并不一定；則本實施例可以從第一估計位姿和第二估計位姿中選中其中更精確的一個作為更新位姿；或者，通過近似估計算法從第一估計位姿和第二估計位姿中提取較精確值而組成更新位姿。例如，近似估計方法可以包括ekf，基于ut變換的卡爾曼濾波(ukf)，粒子濾波，等等。

步驟s106，根據所述更新位姿對所述機器人的位姿進行更新。

本實施例，通過采用慣性導航系統則可以獲得第一估計位姿，進一步采用掃描障礙物的傳感器，則可以不斷的獲得環境地圖；再通過第一估計位姿指導前后環境地圖建立匹配關系，從而可以快速和精確地構建當前環境的地圖；進一步的通過匹配后的前后環境地圖而獲得第二估計位姿，再通過第一估計位姿和第二估計位姿相結合，而獲得更精確的更新位姿，從而使得機器人的定位精度更高，則降低了定位不準而帶來的累計誤差，進而又可以同步構建精度更高當前環境地圖。

請結合參看圖2，本實施例中的步驟s102，所述通過傳感器掃描環境而在預設坐標系中建立參考環境地圖包括：

步驟s201，通過傳感器掃描采集現場環境的障礙物信息的二維點集。其中，本實施例中，傳感器采用的是激光雷達。

步驟s202，將所述二維點集轉換為柵格地圖而在預設坐標系中建立參考環境地圖。其中，預設坐標系可以單個坐標系或同時采用多個坐標系等；坐標系可以是極坐標系或者笛卡爾坐標系等等。

本實施例，通過掃描獲得障礙物的二維點集信息，并且轉化為柵格地圖來構建參考環境地圖，則該方法具有易于實現以及穩定性較高的效果。當然，在其他實施例中，還可以采集更多維的點集，構建向量地圖等等。

請結合參看圖3，本實施例中的步驟s103，所述在移動至下一位置時，通過慣性導航系統預估所述機器人的第一估計位姿包括：

步驟s301，在移動至下一位置時，根據編碼器判別機器人運動距離。

步驟s302，根據電子羅盤與陀螺儀判別機器人運動方向。

步驟s303，根據所述機器人運動距離和運動方向，計算獲得所述機器人當前位姿，并且將所述機器人當前位姿作為所述機器人的第一估計位姿。

本實施例中，通過采用編碼器獲得運動距離，通過電子羅盤和陀螺儀來獲得運動方向，該慣性導航系統具有較為常見而成本低、結構簡單和工作穩定的效果。

請結合參看圖4，本實施例中的步驟s104，所述通過傳感器掃描獲得更新環境地圖，根據所述更新環境地圖、參考環境地圖以及第一估計位姿獲得第二估計位姿以及新參考環境地圖包括：

步驟s401，通過傳感器掃描獲得更新環境地圖。其中，該傳感器與之前傳感器相同，具體為，為機器人本體攜帶的激光掃描儀。

步驟s402，根據所述第一估計位姿，采用迭代最近點算法將所述更新環境地圖映射到所述參考環境地圖上，并且獲得新參考環境地圖。

本步驟中，則是利用第一估計位姿來指導所述更新環境地圖映射至參考環境地圖上，從而參考環境地圖與更新環境地圖的匹配關系，從而獲得新參考環境地圖，達到構建當地環境地圖的效果。進一步的采用迭代算法，能夠較為簡單以及精確的確定映射關系。所述迭代算法在后文中描述。

步驟s403，根據所述參考環境地圖、映射至所述參考環境地圖上的更新環境地圖，以及機器人學空間坐標轉換算法，獲得第二估計位姿。

本實施例中，通過第一估計位姿來指導所述更新環境與參考環境地圖的匹配，并且根據參考環境地圖和更新環境地圖以及機器人學空間左邊轉換算法，來獲得第二估計位姿，則具有降低匹配時的計算難度，達到快速和精確的計算匹配關系的效果；以及通過匹配后的前后環境地圖而獲得的第二估計位姿具有估計較為精確的效果。

具體的，請結合參看圖7，此處對上述公式進行說明：所述擴展卡爾曼濾波：卡爾曼濾波是由卡爾曼本人在1960年提出的一系列數學公式，可以通過觀測系統的輸入和輸出結果，對包含噪聲和干擾的系統狀態進行最優估計，其方法便是利用前一時刻的數據和系統的誤差信息迭代求解當前時刻的狀態信息。卡爾曼濾波器的運用前提是基于線性系統，然而在實際應用中很難保證用線性方程來準確描述系統的狀態情況。系統的過程模型和觀測模型都可能以非線性的形式存在，其非線性模型由如下方程所示，其中f和h代表的是非線性函數。

x(k)＝f(x(k-1),u(k))+w(k)

z(k)＝h(x(k))+v(k)

在實際應用中，非線性模型中的噪聲值我們通常無法獲得，而在將系統線性化的過程中還會引入噪聲，因此可以假設該階段其值為零，將干擾統一考慮在線性化的系統模型中。對上述非線性系統進行線性化的結果如下：

等號左邊分別是實際的系統狀態和觀測變量，等號右邊未加上標的x(k)和z(k)分別是對應的估計值。w(k)和v(k)系統的噪聲干擾，f(k)和h(k)是與狀態轉移和觀測相關的雅可比矩陣。

具體算法過程為：

(1)根據機器人的運動學模型，計算從t-1時刻到t時刻位置的變化(δxt,δyt)以及imu的偏航角變化θt，并計算出t時刻里程計所對應的機器人估計位姿podom(t)＝[x(t),y(t),θ(t)]^t。

(2)令q0＝(δxt,δyt,θt)，根據t時刻激光雷達的掃描數據st以及t-1時刻的參考掃描數據st-1，將q0作為從st-1到st的初始位姿變換，執行plicp算法，迭代計算得到最終的位姿變換qk。再計算t時刻雷達掃描匹配所對應的機器人位姿pscan(t)＝r(θk)pscan(t-1)+tk。

請參看圖5，本實施例以第一實施例為基礎，新增了步驟。具體如下：

步驟s501，與第一實施例的步驟s101相同，在此不再贅述。

步驟s502，與第一實施例的步驟s102相同，在此不再贅述。

步驟s503，與第一實施例的步驟s103相同，在此不再贅述。

步驟s504，與第一實施例的步驟s104相同，在此不再贅述。

步驟s505，與第一實施例的步驟s105相同，在此不再贅述。

步驟s506，與第一實施例的步驟s106相同，在此不再贅述。

步驟s507，通過攝像設備獲得參考路標信息。

步驟s508，在移動至下一位置時，通過攝像設備獲得更新路標信息。

步驟s509，根據所述參考路標信息以及更新路標信息計算獲得卡爾曼增益矩陣，并且根據所述卡爾曼增益矩陣對所述機器人的位姿進行更新。

本實施例，通過新增參考路標特征，則能夠進一步的校準機器人的位姿，達到降低了定位誤差，從而使得同步定位與地圖構建的整體精度進一步提高。

請參看圖6，本發明同步定位與地圖構建設備一實施例。同步定位與地圖構建設備整體為一個四輪驅動機器人，包括：

激光雷達1001，用于發出激光再接收反射激光。在其他實施例中，還可以采用聲波或光波傳感器等。

激光導航模塊1002，用于通過激光雷達接收的信息進行計算，進而對障礙物進行定位。

電子羅盤1008，用于獲取當前機器人的指向。

角度傳感器1009，用于獲取當前機器人的轉動角度。

編碼器1004，用于記錄機器人的移動距離。

數據采集板1003，用于連接上述各個測量模塊與控制板。

控制板1005，用于配置與根據上述測量數值同步定位和構建地圖。

后輪直流無刷電動驅動器1007，用于驅動機器人移動。

前輪步進電機驅動器1006，用于驅動機器人移動。

請結合參看圖7，所述同步定位與地圖構建設備還包括設置在控制板1005上的存儲器、處理器及存儲在所述存儲器上并可在所述處理器上運行的同步定位與地圖構建程序。

所述同步定位與地圖構建程序被所述處理器執行時實現上述實施例中同步定位與地圖構建方法的步驟。具體方案可以參考上述同步定位與地圖構建方法的實施例，在此不再贅述。

需要說明的是，在本文中，術語“包括”、“包含”或者其任何其他變體意在涵蓋非排他性的包含，從而使得包括一系列要素的過程、方法、物品或者裝置不僅包括那些要素，而且還包括沒有明確列出的其他要素，或者是還包括為這種過程、方法、物品或者裝置所固有的要素。在沒有更多限制的情況下，由語句“包括一個……”限定的要素，并不排除在包括該要素的過程、方法、物品或者裝置中還存在另外的相同要素。

上述本發明實施例序號僅僅為了描述，不代表實施例的優劣。

通過以上的實施方式的描述，本領域的技術人員可以清楚地了解到上述實施例方法可借助軟件加必需的通用硬件平臺的方式來實現，當然也可以通過硬件，但很多情況下前者是更佳的實施方式。基于這樣的理解，本發明的技術方案本質上或者說對現有技術做出貢獻的部分可以以軟件產品的形式體現出來，該計算機軟件產品存儲在一個存儲介質(如rom/ram、磁碟、光盤)中，包括若干指令用以使得一臺終端(可以是手機，計算機，服務器，空調器，或者網絡設備等)執行本發明各個實施例所述的方法。

上面結合附圖對本發明的實施例進行了描述，但是本發明并不局限于上述的具體實施方式，上述的具體實施方式僅僅是示意性的，而不是限制性的，本領域的普通技術人員在本發明的啟示下，在不脫離本發明宗旨和權利要求所保護的范圍情況下，還可做出很多形式，這些均屬于本發明的保護之內。