地圖構建方法及機器人控制系統與流程

本發明涉及計算機領域，尤其涉及一種地圖構建方法及機器人控制系統。
背景技術：
：在移動機器人領域，slam(simultaneouslocalizationandmapping是一個非常基礎而關鍵的問題，是機器人學領域最熱門的研究方向之一，在過去的近30年中得到了廣泛的關注與研究。slam研究領域的學者們提出了大量的slam方法，包括各種各樣的傳感器、優化技術和地圖描述。獲得機器人工作空間的地圖是實現定位、避障、導航以及自主性任務的關鍵問題，尤其當移動機器人工作在復雜、動態環境中，僅僅使用在板傳感器快速地生成并維護工作空間的3維地圖是非常重要的。激光掃描儀能夠以高頻率提供精確的環境的幾何測量，許多先進的slam系統結合各種改進的icp(iterativeclosestpoint)算法，計算觀測之間的相對運動。在移動機器人上配備機械臂可以通過2維激光掃描儀實現精確的3維定位，結合imu能夠在移動機器人平臺上實現地圖構建，通過兩個激光掃描儀和icp算法能夠以毫米級的精度估計機器人位姿。但是，激光掃描儀存在笨重、昂貴和獲取信息量少的問題，此外，icp的缺陷包括依賴于良好的初始化估計以防止局部最小值和缺乏整體匹配質量的衡量。激光掃描儀存在的主要不足是價格高、耗能大和體積重量大，如何通過更易于實現的方式實現地圖構建，是當前需要解決的技術問題。技術實現要素：本發明提供一種地圖構建方法及機器人控制系統，能夠更易于實現地圖構建。本發明實施例采用如下技術方案：一種地圖構建方法，包括：提取當前幀的特征點；獲取參考幀的特征點，根據所述當前幀的特征點及所述參考幀的特征點，得出當前位姿；通過幀間配準獲取的位姿估計存在累積誤差，將局部地圖中的關鍵幀的特征點投影到所述當前幀；當所述前幀符合新增關鍵幀的條件時，向所述局部地圖插入關鍵幀。可選的，所述提取當前幀的特征點包括：對所述當前幀采用orb算法檢測特征點并計算特征點的方向，通過隨機選取特征點鄰域內的像素點對進行比較獲得所述特征點二進制描述符；所述根據所述當前幀的特征點及所述參考幀的特征點，得出當前位姿包括：根據運動模型預估所述參考幀的特征點在所述當前幀的區域內利用所述二進制描述符搜索匹配點，并通過方向一致性校驗優化初始對應點，通過幀間匹配計算出所述當前位姿。可選的，還包括：所述局部地圖構建過程中檢測冗余的關鍵幀并刪除。可選的，還包括：根據所述局部地圖優化所述關鍵幀的位姿，確定所述關鍵幀作為候選回環關鍵幀；計算新插入的關鍵幀與閉環關鍵幀之間的相似變換，確定閉環檢測和閉環關鍵幀；根據確定的閉環關鍵幀，在互可見圖中插入新的邊以連接閉環，融合重復的地圖點。可選的，所述根據所述局部地圖優化所述關鍵幀的位姿，確定所述關鍵幀作為候選回環關鍵幀包括：根據局部地圖優化所述關鍵幀ki的位姿，檢測新插入的所述關鍵幀ki與其它關鍵幀是否構成回路閉合；基于視覺詞袋模型計算所述關鍵幀ki和互可見圖中相鄰的關鍵幀之間的相似性，記錄最低得分smin，使用同樣的方法計算所述關鍵幀ki與地圖中其它關鍵幀之間的相似性，忽略所有得分低于smin的關鍵幀，在某個關鍵幀的互可見圖中連續檢測出3個候選回環關鍵幀，即確定該關鍵幀作為候選回環關鍵幀；所述計算新插入的關鍵幀與閉環關鍵幀之間的相似變換，確定閉環檢測和閉環關鍵幀，包括：檢測出候選閉環關鍵幀后，計算新插入的關鍵幀與閉環關鍵幀之間的相似變換，所述相似變換即回環中的累積誤差，計算當前關鍵幀和候選回環關鍵幀地圖點之間的對應關系，得到一組3d到3d的對應點，采用ransac迭代計算相似變換，達到擁有設定數量的內點，則確定閉環檢測和閉環關鍵幀；所述根據確定的閉環關鍵幀，在互可見圖中插入新的邊以連接閉環，融合重復的地圖點，包括：根據確定的閉環關鍵幀，在互可見圖中插入新的邊以連接閉環，融合重復的地圖點，更正當前關鍵幀的位姿，進而將更正傳遞到閉合環路中的所有關鍵幀以及地圖點，所有的關鍵幀都將產生新的邊以將重新優化后的關鍵幀連接成回環，消除回環中的累積誤差。可選的，還包括：根據所述關鍵幀，生成點云地圖；根據所述點云地圖構建3d環境的八叉樹地圖。可選的，所述根據所述關鍵幀，生成點云地圖包括：確定空間位置[x,y,z]和像素坐標[u,v,d]之間的對應關系：d＝z·s其中，fx、fy、cx和cy是攝像機內部參數，通過校準攝像機校準獲得，s表示深度縮放因子，d表示圖像深度。那么，可以推導出像素在空間中的位置，采用矩陣的表達方式：其中，c為攝像機內參矩陣，r和t分別表示攝像機的旋轉和位移。可選的，所述根據所述點云地圖構建3d環境的八叉樹地圖，包括：八叉樹中的葉子，用概率的形式表示是否被占據，對于葉子節點n，在1,2,…t時刻的觀測數據為z1,z2,...zt，則該節點記錄的信息：其中，p(n)一般情況下假設為0.5，此處引入logit變換：兩邊取logit變換，代入p(n)＝0.5，化簡后得：l(n|z1:t)＝l(n|z1:t-1)+l(n|zt)對于任意空間中任意一點，每次獲得新的觀測能夠直接加到原來的上面，因此更新方式靈活，父節點可以根據子節點的數值計算其占據概率，主要有取平均值和取最大值兩種方式，因此基于八叉樹地圖庫能夠在生成點云圖的過程中，實時的構建3d環境的八叉樹地圖。一種機器人控制系統，包括：傳感器、計算機、控制板、機器人；所述傳感器獲取距離參數；所述控制板用于控制所述機器人；所述計算機用于執行：提取當前幀的特征點；獲取參考幀的特征點，根據所述當前幀的特征點及所述參考幀的特征點，得出當前位姿；通過幀間配準獲取的位姿估計存在累積誤差，將局部地圖中的關鍵幀的特征點投影到所述當前幀；當所述前幀符合新增關鍵幀的條件時，向所述局部地圖插入關鍵幀。可選的，所述計算機還用于執行：所述局部地圖構建過程中檢測冗余的關鍵幀并刪除；所述計算機還用于執行：根據所述局部地圖優化所述關鍵幀的位姿，確定所述關鍵幀作為候選回環關鍵幀；計算新插入的關鍵幀與閉環關鍵幀之間的相似變換，確定閉環檢測和閉環關鍵幀；根據確定的閉環關鍵幀，在互可見圖中插入新的邊以連接閉環，融合重復的地圖點；所述計算機還用于執行：根據所述關鍵幀，生成點云地圖；根據所述點云地圖構建3d環境的八叉樹地圖。本發明實施例提供的地圖構建方法及機器人控制系統，提取當前幀的特征點；獲取參考幀的特征點，根據當前幀的特征點及參考幀的特征點，得出當前位姿；通過幀間配準獲取的位姿估計存在累積誤差，將局部地圖中的關鍵幀的特征點投影到當前幀；當前幀符合新增關鍵幀的條件時，向局部地圖插入關鍵幀。從而能夠更易于實現地圖構建。附圖說明此處的附圖被并入說明書中并構成本說明書的一部分，示出了符合本發明的實施例，并與說明書一起解釋本發明的原理。圖1為本發明實施例提供的一種地圖構建方法的流程圖。圖2為本發明實施例提供的一種機器人控制系統的結構示意圖。圖3為本發明實施例提供的一種機器人控制系統的中傳感器成像示意圖。圖4為本發明實施例提供的一種機器人控制系統的控制板示意圖。具體實施方式下面將結合本發明實施例中的附圖，對本發明實施例中的技術方案進行清楚、完整地描述，顯然，所描述的實施例僅僅是本發明一部分實施例，而不是全部的實施例。基于本發明中的實施例，本領域普通技術人員在沒有做出創造性勞動前提下所獲得的所有其他實施例，都屬于本發明保護的范圍。實施例1本發明實施例提供一種地圖構建方法，如圖1所示，該方法包括：11、提取當前幀的特征點；12、獲取參考幀的特征點，根據所述當前幀的特征點及所述參考幀的特征點，得出當前位姿；13、通過幀間配準獲取的位姿估計存在累積誤差，將局部地圖中的關鍵幀的特征點投影到所述當前幀；14、當所述前幀符合新增關鍵幀的條件時，向所述局部地圖插入關鍵幀。可選的，所述提取當前幀的特征點包括：對所述當前幀采用orb(orientedfastandrotatedbrief)算法檢測特征點并計算特征點的方向，通過隨機選取特征點鄰域內的像素點對進行比較獲得所述特征點二進制描述符。具體的，對當前幀采用orb算法檢測特征點并計算特征點的方向，方向信息在關鍵點特征描述符的匹配中非常有用，通過隨機選取特征點鄰域內的像素點對進行比較獲得該特征點二進制描述符。視覺特征點的提取和描述基于opencv庫實現，其中每一幀提取1000個特征點，尺度因子為1.2，尺度分層數為8。所述根據所述當前幀的特征點及所述參考幀的特征點，得出當前位姿包括：根據運動模型預估所述參考幀的特征點在所述當前幀的區域內利用所述二進制描述符搜索匹配點，并通過方向一致性校驗優化初始對應點，通過幀間匹配計算出所述當前位姿。具體的，當前幀的特征點與參考幀的特征點存在對應關系，根據運動模型預估參考幀的特征點在當前幀的區域，在該區域內利用上一步驟計算的二進制描述符搜索匹配點，并通過方向一致性校驗優化初始對應點。至此，獲得一系列3d到2d的對應關系，在ransac方案中解決pnp(perspective-n-point)問題，通過幀間匹配計算出攝像機的當前位姿。可選的，所述通過幀間配準獲取的位姿估計存在累積誤差，將局部地圖中的關鍵幀的特征點投影到所述當前幀包括：通過幀間配準獲取的位姿估計存在累積誤差，將局部地圖中的關鍵幀的特征點投影到當前幀，構造優化特征估計位姿的殘差。優化后得到的特征點的位置，比之前通過攝像機位姿估計的更精確，由此可優化地圖中的特征點位置和攝像機的位姿。可選的，還包括：所述局部地圖構建過程中檢測冗余的關鍵幀并刪除。具體的，精確的局部地圖有利于優化幀間匹配的累積誤差，當前幀符合新增關鍵幀的條件，則向局部地圖插入關鍵幀并采用視覺詞袋模型進行描述。檢查新插入的關鍵幀中的地圖點在相關的關鍵幀中的重投影誤差，忽略不能滿足極線約束的錯誤匹配。剔除不符合要求的地圖點以提高算法的魯棒性，減少存在誤差的地圖點對位姿估計的優化造成不利影響。采用局部捆集調整(bundleadjustment，ba)優化新的局部地圖，包括新插入的關鍵幀及其相關聯的關鍵幀，以及這些關鍵幀中的全部地圖點。由于ba的復雜度是關鍵幀數量的三次方，局部地圖構建過程中檢測冗余的關鍵幀并刪除。在相同的場景下，關鍵幀的數量不會隨著攝像機的重復運動而無限制的增加。關鍵幀的增加僅在攝像機進入新的場景時而產生，有利于長時間的構圖，刪除冗余關鍵幀能夠有效提升算法的實時性又能保持魯棒性。可選的，還包括：根據所述局部地圖優化所述關鍵幀的位姿，確定所述關鍵幀作為候選回環關鍵幀；計算新插入的關鍵幀與閉環關鍵幀之間的相似變換，確定閉環檢測和閉環關鍵幀；根據確定的閉環關鍵幀，在互可見圖中插入新的邊以連接閉環，融合重復的地圖點。可選的，所述根據所述局部地圖優化所述關鍵幀的位姿，確定所述關鍵幀作為候選回環關鍵幀包括：根據局部地圖優化所述關鍵幀ki的位姿，檢測新插入的所述關鍵幀ki與其它關鍵幀是否構成回路閉合；基于視覺詞袋模型計算所述關鍵幀ki和互可見圖中相鄰的關鍵幀之間的相似性，記錄最低得分smin，使用同樣的方法計算所述關鍵幀ki與地圖中其它關鍵幀之間的相似性，忽略所有得分低于smin的關鍵幀，在某個關鍵幀的互可見圖中連續檢測出3個候選回環關鍵幀，即確定該關鍵幀作為候選回環關鍵幀。具體的，根據局部地圖優化關鍵幀ki的位姿，尤其是大尺度范圍中，仍存在誤差累積問題。檢測新插入的關鍵幀ki與其它關鍵幀是否構成回路閉合，二者位姿偏差即為累積的誤差，進而優化閉合回路中的所有關鍵幀。首先，基于視覺詞袋模型計算ki和互可見圖中相鄰的關鍵幀之間的相似性，記錄最低得分smin。然后，使用同樣的方法計算ki與地圖中其它關鍵幀之間的相似性，忽略所有得分低于smin的關鍵幀。最后，在某個關鍵幀的互可見圖中連續檢測出3個候選回環關鍵幀，即確定該關鍵幀作為候選回環關鍵幀。所述計算新插入的關鍵幀與閉環關鍵幀之間的相似變換，確定閉環檢測和閉環關鍵幀，包括：檢測出候選閉環關鍵幀后，計算新插入的關鍵幀與閉環關鍵幀之間的相似變換，所述相似變換即回環中的累積誤差，計算當前關鍵幀和候選回環關鍵幀地圖點之間的對應關系，得到一組3d到3d的對應點，采用ransac迭代計算相似變換，達到擁有設定數量的內點，則確定閉環檢測和閉環關鍵幀。具體的，檢測出候選閉環關鍵幀后，計算新插入的關鍵幀與閉環關鍵幀之間的相似變換，該相似變換即回環中的累積誤差。計算當前關鍵幀和候選回環關鍵幀地圖點之間的對應關系，得到一組3d到3d的對應點，采用ransac迭代計算相似變換。如果擁有足夠的內點，那么就能確定閉環檢測和閉環關鍵幀。所述根據確定的閉環關鍵幀，在互可見圖中插入新的邊以連接閉環，融合重復的地圖點，包括：根據確定的閉環關鍵幀，在互可見圖中插入新的邊以連接閉環，融合重復的地圖點，更正當前關鍵幀的位姿，進而將更正傳遞到閉合環路中的所有關鍵幀以及地圖點，所有的關鍵幀都將產生新的邊以將重新優化后的關鍵幀連接成回環，消除回環中的累積誤差。具體的，根據確定的閉環關鍵幀，在互可見圖中插入新的邊以連接閉環，融合重復的地圖點。更正當前關鍵幀的位姿，進而將更正傳遞到閉合環路中的所有關鍵幀以及地圖點。所有的關鍵幀都將產生新的邊以將重新優化后的關鍵幀連接成回環，消除了回環中的累積誤差。可選的，還包括：根據所述關鍵幀，生成點云地圖；根據所述點云地圖構建3d環境的八叉樹地圖。可選的，所述根據所述關鍵幀，生成點云地圖包括：確定空間位置[x,y,z]和像素坐標[u,v,d]之間的對應關系：d＝z·s其中，fx、fy、cx和cy是攝像機內部參數，通過校準攝像機校準獲得，s表示深度縮放因子，d表示圖像深度。那么，可以推導出像素在空間中的位置，采用矩陣的表達方式：其中，c為攝像機內參矩陣，r和t分別表示攝像機的旋轉和位移。具體的，上述稀疏地圖主要為提升定位的精度和魯棒性服務，而不適合于避障等應用，因此在高精度和高魯棒性的關鍵幀的基礎之上，采用點云圖和八叉樹地圖技術，構建適合機器人導航等應用的3d網格地圖。首先，3d環境可由一系列點描述：x＝{x1,x2,……,xn}，其中xi＝[r,g,b,x,y,z]，分別表示該點的顏色和空間位置共6個元素。對于rgb-d傳感器而言，顏色信息存儲于彩色圖像，絕對尺度的空間位置可以通過針孔攝像機模型和深度圖像的數據推算獲得。如圖2所示的針孔攝像機模型，根據幾何關系可得空間位置[x,y,z]和像素坐標[u,v,d]之間的對應關系：d＝z·s其中，fx、fy、cx和cy是攝像機內部參數，通過校準攝像機校準獲得，s表示深度縮放因子，d表示圖像深度。那么，可以推導出像素在空間中的位置，采用矩陣的表達方式：其中，c為攝像機內參矩陣，r和t分別表示攝像機的旋轉和位移。可選的，所述根據所述點云地圖構建3d環境的八叉樹地圖，包括：八叉樹地圖能夠描述任意形式的3d環境，能夠以概率的形式更新，具有更新方式靈活、無需先驗知識和地圖形式非常緊湊等優點。八叉樹中的葉子，用概率的形式表示是否被占據，對于葉子節點n，在1,2,…t時刻的觀測數據為z1,z2,...zt，則該節點記錄的信息：其中，p(n)一般情況下假設為0.5，此處引入logit變換：兩邊取logit變換，代入p(n)＝0.5，化簡后得：l(n|z1:t)＝l(n|z1:t-1)+l(n|zt)由上式可知，對于任意空間中任意一點，每次獲得新的觀測能夠直接加到原來的上面，因此更新方式靈活。父節點可以根據子節點的數值計算其占據概率，主要有取平均值和取最大值兩種方式，因此基于八叉樹地圖庫能夠在生成點云圖的過程中，實時的構建3d環境的八叉樹地圖。本發明實施例提供的地圖構建方法，提取當前幀的特征點；獲取參考幀的特征點，根據當前幀的特征點及參考幀的特征點，得出當前位姿；通過幀間配準獲取的位姿估計存在累積誤差，將局部地圖中的關鍵幀的特征點投影到當前幀；當前幀符合新增關鍵幀的條件時，向局部地圖插入關鍵幀。從而能夠更易于實現地圖構建。實施例2本發明實施例提供一種機器人控制系統，該機器人控制系統可以應用上述實施例1提供的方法。如圖3所示，該系統包括：傳感器21、計算機22、控制板23、機器人24；所述傳感器21獲取距離參數；所述控制板23用于控制所述機器人；所述計算機22用于執行：提取當前幀的特征點；獲取參考幀的特征點，根據所述當前幀的特征點及所述參考幀的特征點，得出當前位姿；通過幀間配準獲取的位姿估計存在累積誤差，將局部地圖中的關鍵幀的特征點投影到所述當前幀；當所述前幀符合新增關鍵幀的條件時，向所述局部地圖插入關鍵幀。可選的，所述計算機22還用于執行：所述局部地圖構建過程中檢測冗余的關鍵幀并刪除；所述計算機還22用于執行：根據所述局部地圖優化所述關鍵幀的位姿，確定所述關鍵幀作為候選回環關鍵幀；計算新插入的關鍵幀與閉環關鍵幀之間的相似變換，確定閉環檢測和閉環關鍵幀；根據確定的閉環關鍵幀，在互可見圖中插入新的邊以連接閉環，融合重復的地圖點；所述計算機22還用于執行：根據所述關鍵幀，生成點云地圖；根據所述點云地圖構建3d環境的八叉樹地圖。本發明實施例的計算機22的詳細功能可以參照上述實施例1，此處不贅述。在一個實施例中，該機器人控制系統可以采用如下平臺硬件設計：傳感器21采用硬件kinect2.0、計算機22采用微型計算機(例如i7處理器的微型計算機。)、控制板23采用pixhawk、機器人23為移動機器人，移動機器人平臺基座為axialax10。可選的，移動機器人平臺的基座為ax-10攀爬車的底座，具備多功能爬行能力和靈活的速度控制特性，由ae-2esc和20t電機提供動力，as-3金屬齒輪伺服系統安全且精確的控制。移動機器人平臺底座見相關參數如下表：規格數值軸距(mm)307長度(mm)467.4寬度(mm)295.3高度(mm)238離地間隙(mm)76.2重量(kg)1.9可選的，微軟的kinect2.0傳感器提供了一個基于在板的tof距離傳感器。kinect2.0由多個傳感器組成，包括rgb數字攝像機、麥克風和3dtof距離攝像機傳感器，能夠提供深度圖像，相關參數如下表所示：規格數值rgb攝像機像素精度1920×1080tof攝像機像素精度512×424幀速率30fpstof傳感器深度距離(m)0.5-4.5傳感器尺寸(cm)25×6.5×6.5可選的，主控計算機是移動機器人平臺視覺定位與地圖構建算法以及自主控制的核心，它主要處理視覺傳感器的數據并對整個平臺進行控制，要求處理能力強和運行速度快。龍浩微動研發的高性能、低功耗、低輻射、全功能單板計算機，其硬件參數和處理運行速度能夠滿足移動機器人實時運行的要求，且能完成定位、地圖構建、障礙物檢測和路徑規劃等高級任務的計算需求，同時具有足夠的存儲空間來記錄數據。下表為本文主控計算機的配置硬件參數，主控計算機通過usb3.0接口與kinect2.0連接，通過usb2.0接口與pixhawk建立連接，在板主控計算機搭載ubuntu14.04lts操作系統、indigo版的機器人操作系統(ros)和opencv(2.4.11)圖像處理庫。可選的，pixhawk控制板控制部分是執行命令和驅動控制的樞紐，主控計算機接收kinect2.0傳感器數據，通過vslam算法構建環境的3d地圖同時估計機器人平臺的當前位姿估計。pixhawk控制板是一個高性能的自動駕駛模塊，兼容固定翼飛行器、多旋翼飛行器、直升機、車輛和船舶等移動機器人平臺。pixhawk控制板是一款開源的微控制平臺，裝配了具有強處理能力的stm32f427cortexm4168mhz微處理器，ram為256kb，flash存儲為2mb。除了主要的cpu外，還有一個stm32f103處理器，避免主處理器失效時機器人平臺的失控。為了拓展自主能力，pixhawk控制板提供了多個接口來連接外部傳感器，如圖4所示。pixhawk主要參數如下本發明實施例提供的機器人控制系統，提取當前幀的特征點；獲取參考幀的特征點，根據當前幀的特征點及參考幀的特征點，得出當前位姿；通過幀間配準獲取的位姿估計存在累積誤差，將局部地圖中的關鍵幀的特征點投影到當前幀；當前幀符合新增關鍵幀的條件時，向局部地圖插入關鍵幀。從而能夠更易于實現地圖構建。以上，僅為本發明的具體實施方式，但本發明的保護范圍并不局限，任何熟悉本
技術領域：
的技術人員在本發明揭露的技術范圍內，可輕易想到變化或替換，都應涵蓋在本發明的保護范圍之內。因此，本發明的保護范圍應以權利要求的保護范圍為準。當前第1頁12