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