(19)國家知識(shí)產(chǎn)權(quán)局(12)發(fā)明專利(10)授權(quán)公告號(hào)CN114723920B(65)同一申請(qǐng)的已公布的文獻(xiàn)號(hào)(73)專利權(quán)人南京大學(xué)地址210023江蘇省南京市棲霞區(qū)仙林大道163號(hào)南京大學(xué)專利權(quán)人江蘇圖客機(jī)器人有限公司鄢偉(74)專利代理機(jī)構(gòu)江蘇圣典律師事務(wù)所32237專利代理師胡建華于瀚文審查員朱淼一種基于點(diǎn)云地圖的視覺定位方法本發(fā)明提供了一種基于點(diǎn)云地圖的視覺定位方法，包括點(diǎn)云地圖生成模塊、視覺慣性里程計(jì)構(gòu)建模塊，以及基于點(diǎn)云地圖的視覺匹配定位GPS信息建立高精度的點(diǎn)云地圖；視覺慣性里程計(jì)構(gòu)建模塊先提取視覺特征點(diǎn)，使用光流法追蹤前后幀特征點(diǎn)，并對(duì)IMU進(jìn)行預(yù)積分，和視覺特征點(diǎn)進(jìn)行融合，構(gòu)建視覺慣性里程計(jì)，輸出每一幀的初始位姿，并恢復(fù)特征點(diǎn)的深度；基于已有地圖的視覺匹配定位模塊根據(jù)初始位置從點(diǎn)云地圖中提取子圖，通過視覺恢復(fù)的特征點(diǎn)投影到3D空間地圖坐標(biāo)系下，并在當(dāng)前子圖中查詢最近點(diǎn)。對(duì)于視覺和地圖匹配到的最近點(diǎn)，采用基于激光視覺特征構(gòu)建定位激光點(diǎn)云地圖21.一種基于點(diǎn)云地圖的視覺定位方法，其特征在于，包括：建立點(diǎn)云地圖構(gòu)建模塊、視覺慣性里程計(jì)構(gòu)建模塊和視覺匹配定位模塊；融合激光建圖；所述視覺慣性里程計(jì)構(gòu)建模塊基于點(diǎn)云地圖的視覺定位提供初始位姿，通過融合單目視覺和慣性測量單元IMU,求解每一幀圖像的位姿和特征點(diǎn)深度；所述視覺匹配定位模塊基于不同模態(tài)的點(diǎn)云和視覺信息，對(duì)地圖點(diǎn)構(gòu)建kd-tree,與投影到地圖坐標(biāo)系的特征點(diǎn)匹配后采用基于對(duì)偶四元數(shù)的粒子濾波器進(jìn)行定位；所述點(diǎn)云地圖構(gòu)建模塊具體執(zhí)行如下步驟：步驟1-1,以激光為基準(zhǔn)，將慣性測量單元IMU和GPS數(shù)據(jù)相對(duì)于激光進(jìn)行對(duì)齊，對(duì)于每一幀激光點(diǎn)云，使用慣性測量單元IMU進(jìn)行去畸變處理；步驟1-2,對(duì)慣性測量單元IMU進(jìn)行慣性解算，解算結(jié)果作為卡爾曼濾波的預(yù)測更新；步驟1-3,對(duì)于相鄰兩幀的激光點(diǎn)云，采用最近點(diǎn)迭代ICP計(jì)算相對(duì)位姿，作為初始位步驟1-4,進(jìn)行卡爾曼濾波預(yù)測更新；步驟1-5,當(dāng)有新的激光點(diǎn)云幀時(shí)，融合機(jī)器人運(yùn)動(dòng)約束，進(jìn)行觀測更新；步驟1-6,將當(dāng)前后驗(yàn)位姿作為下一次的先驗(yàn)，不斷迭代求解新的后驗(yàn)位姿，直到最近兩次位姿變化不超過第一閾值，即位姿收斂；步驟1-7,將前三幀均作為關(guān)鍵幀，當(dāng)當(dāng)前幀的位姿和上一關(guān)鍵幀的距離相差超過第二閾值時(shí)，將當(dāng)前幀設(shè)置為關(guān)鍵幀，作為優(yōu)化節(jié)點(diǎn)加入圖優(yōu)化中，圖優(yōu)化將連續(xù)N?幀關(guān)鍵幀的姿態(tài)作為優(yōu)化變量，關(guān)鍵幀中觀測到的像素特征點(diǎn)作為圖優(yōu)化的邊，特征點(diǎn)的投影誤差作為圖的代價(jià)，通過增大或減小圖中節(jié)點(diǎn)的姿態(tài)，使得圖的代價(jià)最?。徊襟E1-8,當(dāng)滿足以下條件：當(dāng)前累積關(guān)鍵幀數(shù)目超過X?幀或累積GPS數(shù)據(jù)超過X?幀，或步驟1-9,對(duì)于關(guān)鍵幀，將關(guān)鍵幀點(diǎn)云轉(zhuǎn)換到地圖坐標(biāo)系下，將第一幀點(diǎn)云的位姿作為所述視覺慣性里程計(jì)構(gòu)建模塊具體執(zhí)行如下步驟：步驟2-1,預(yù)處理階段：在視覺相機(jī)每一幀中提取Harris角點(diǎn)，作為視覺特征點(diǎn)，使用Lukas-Kanade光流追蹤相鄰幀的特征點(diǎn)，并使用RANSAC去除異常追蹤點(diǎn)，同時(shí)對(duì)慣性測量步驟2-2,初始化階段：僅采用視覺恢復(fù)前面一定幀的位姿和特征點(diǎn)深度，最后和IMU預(yù)積分對(duì)齊求解深度；步驟2-3,后端優(yōu)化階段：將慣性測量單元IMU的誤差和視覺誤差一起進(jìn)行非線性優(yōu)化，優(yōu)化每一幀的位置、速度、旋轉(zhuǎn)和慣性測量單元IMU參數(shù)、特征點(diǎn)位置，輸出值，輸出連續(xù)的位姿和視覺特征作為視覺慣性里程計(jì)；所述視覺匹配定位模塊具體執(zhí)行如下步驟：步驟3-1,對(duì)點(diǎn)云地圖進(jìn)行預(yù)處理，在匹配前對(duì)點(diǎn)云地圖進(jìn)行廣度優(yōu)先搜索BFS聚類，篩去點(diǎn)數(shù)少于X?個(gè)的點(diǎn)云簇，將整張點(diǎn)云地圖分割為長方體的網(wǎng)格，長方體網(wǎng)格的尺寸為40m*40m*40m,并以當(dāng)前位置所在網(wǎng)格為中心構(gòu)建子圖，子圖為5*5*3的網(wǎng)格；3步驟3-2,對(duì)于當(dāng)前相機(jī)視覺幀的特征點(diǎn)，在視覺慣性里程計(jì)中已經(jīng)對(duì)其深度進(jìn)行估計(jì)，將特征點(diǎn)根據(jù)當(dāng)前幀位姿投影到地圖坐標(biāo)系下，使用kd-tree算法在當(dāng)前子圖中搜索最近的地圖點(diǎn)作為匹配對(duì)應(yīng)點(diǎn)；步驟3-3,設(shè)定步驟3-2中視覺特征點(diǎn)在地圖中的對(duì)應(yīng)點(diǎn)對(duì)數(shù)為N,將N對(duì)點(diǎn)隨機(jī)排列，每八對(duì)點(diǎn)作為一組，則對(duì)應(yīng)點(diǎn)分成N/8組對(duì)應(yīng)點(diǎn)集合；平移表示的四元數(shù)，為對(duì)偶數(shù)，滿足ε2=0;設(shè)定步驟3-2中正確的匹配點(diǎn)對(duì)比例為X?,步驟3-3中8對(duì)點(diǎn)的集合作為局內(nèi)點(diǎn)，采用RANSAC算法估計(jì)出當(dāng)前的位姿；步驟3-5,使用當(dāng)前的位姿將視覺特征點(diǎn)投影到地圖坐標(biāo)系，并查詢最近點(diǎn)對(duì)，重復(fù)步驟3-3～步驟3-4,直到最新估計(jì)的位姿旋轉(zhuǎn)角度變化不超過第三閾值，且平移不超過第四閾值，或者迭代次數(shù)超過第五閾值，輸出最終的位姿作為定位結(jié)果。2.根據(jù)權(quán)利要求1所述的方法，其特征在于，步驟1-2中，采用如下公式進(jìn)行慣性解算：其中，其中R為k時(shí)刻慣性測量單元IMU的先驗(yàn)旋轉(zhuǎn)量，R是k-1時(shí)刻的后驗(yàn)旋轉(zhuǎn)量，0時(shí)刻的后驗(yàn)旋轉(zhuǎn)量為單位矩陣，φ是慣性測量單元IMU的旋轉(zhuǎn)向量，上標(biāo)人符號(hào)表示對(duì)向量取反對(duì)稱矩陣，k為k時(shí)刻的先驗(yàn)速度，A-1為k-1時(shí)刻的后驗(yàn)速度，0時(shí)刻的后驗(yàn)速度為0,a為k時(shí)慣性測量單元IMU間隔時(shí)間，Pk為k時(shí)刻IMU的先驗(yàn)位置，P為k-1時(shí)刻慣性測量單元IMU的后驗(yàn)位置，@為IMU角速度。3.根據(jù)權(quán)利要求2所述的方法，其特征在于，步驟1-4包括：采用如下公式計(jì)算先驗(yàn)位δxx=Fk_δxk-1+B_@所示：4采用如下公式計(jì)算觀測量y:n=[n8p.nsp,n8p.nz,n?nsonso,5P=(I-KGA)PPk=Pk-δPR,=R(I-(δ0)^)6一種基于點(diǎn)云地圖的視覺定位方法技術(shù)領(lǐng)域[0001]本發(fā)明涉及機(jī)器人定位領(lǐng)域，特別涉及一種基于點(diǎn)云地圖的視覺定位方法。背景技術(shù)[0002]隨著智能芯片、5G通信、人工智能技術(shù)的快速發(fā)展，智能機(jī)器人已經(jīng)廣泛應(yīng)用于各種環(huán)境，如工業(yè)領(lǐng)域搬運(yùn)機(jī)器人、商業(yè)領(lǐng)域送餐機(jī)器人、家用掃地機(jī)器人。定位是機(jī)器人技術(shù)的核心，實(shí)現(xiàn)機(jī)器人自身的準(zhǔn)確定位是一項(xiàng)最基本、最重要的功能。定位模塊是機(jī)器人完進(jìn)行準(zhǔn)確的定位。目前主流的定位方案有兩種，基于激光的方案以及基于視覺的方案?；诩す獾亩ㄎ环桨篙^為成熟，精度普遍比視覺高，但是激光的價(jià)格昂貴，而且激光在垂直方向的線束有限，獲取的信息較少，影響定位精度；基于視覺的定位方案穩(wěn)定性不如基于激光的方案，在特征紋理較少以及場景變化較大的動(dòng)態(tài)環(huán)境魯棒性較差。[0004]根據(jù)上述介紹，當(dāng)前機(jī)器人定位領(lǐng)域仍存在以下問題：1)基于視覺的方案對(duì)場景變化敏感，定位精度差，且僅依靠單目相機(jī)無法求解尺度。2)基于激光的方案成本較高，難以落地。將視覺和激光融合雖然提高了精度，但是在成本和系統(tǒng)資源占用上存在劣勢(shì)。本文針對(duì)以上問題，提出了基于點(diǎn)云地圖的視覺慣性定位，在已知點(diǎn)云地圖時(shí)，僅使用相機(jī)和IMU,提高了定位的精度，降低了定位成本。發(fā)明內(nèi)容[0005]發(fā)明目的：本發(fā)明要解決的問題為針對(duì)當(dāng)前定位方案精度不足，成本較高等問題。提出了基于已知地圖的視覺慣性定位方法。融合激光、IMU和GPS(全球定位系統(tǒng)，GlobalPositioningSystem)建立高精度地圖，并將地圖和視覺慣性里程計(jì)恢復(fù)的特征點(diǎn)進(jìn)行匹配，優(yōu)化機(jī)器人位姿，降低硬件成本的同時(shí)提高了定位精度，使得系統(tǒng)的魯棒性得以保證。[0006]為了解決上述技術(shù)問題，本發(fā)明公開了一種基于點(diǎn)云地圖的視覺慣性定位方法，首先建立點(diǎn)云地圖構(gòu)建模塊、視覺慣性里程計(jì)構(gòu)建模塊和基于點(diǎn)云地圖的定位模塊。其中點(diǎn)云地圖構(gòu)建模塊提供定位所需地圖，視覺慣性里程計(jì)構(gòu)建模塊為定位模塊提供初始位[0007]本發(fā)明方法具體包括：建立點(diǎn)云地圖構(gòu)建模塊、視覺慣性里程計(jì)構(gòu)建模塊和視覺匹配定位模塊；[0008]所述點(diǎn)云地圖構(gòu)建模塊用于，將慣性測量單元IMU、激光和GPS(全球定位系統(tǒng))進(jìn)行融合，實(shí)現(xiàn)多傳感器融合激光建圖；[0009]所述視覺慣性里程計(jì)構(gòu)建模塊基于點(diǎn)云地圖的視覺定位提供初始位姿，通過融合單目視覺和慣性測量單元IMU,求解每一幀圖像的位姿和特征點(diǎn)深度；[0010]所述視覺匹配定位模塊基于不同模態(tài)的點(diǎn)云和視覺信息，對(duì)地圖點(diǎn)構(gòu)建kd-tree7(k維樹，k為數(shù)據(jù)的維度),與投影到地圖坐標(biāo)系的特征點(diǎn)匹配后采用基于對(duì)偶四元數(shù)的粒子濾波器進(jìn)行定位。[0011]所述點(diǎn)云地圖構(gòu)建模塊具體執(zhí)行如下步驟：[0012]步驟1-1,以激光為基準(zhǔn)，將慣性測量單元IMU和GPS數(shù)據(jù)相對(duì)于激光進(jìn)行對(duì)齊，對(duì)于每一幀激光點(diǎn)云，使用100Hz的慣性測量單元IMU進(jìn)行去畸變處理；[0013]步驟1-2,對(duì)慣性測量單元IMU進(jìn)行慣性解算，解算結(jié)果作為卡爾曼濾波的預(yù)測更[0014]步驟1-3,對(duì)于相鄰兩幀的激光點(diǎn)云，采用最近點(diǎn)迭代ICP計(jì)算相對(duì)位姿，作為初始位姿，用于后續(xù)卡爾曼濾波的觀測更新；[0015]步驟1-4,進(jìn)行卡爾曼濾波預(yù)測更新；[0017]步驟1-6,將當(dāng)前后驗(yàn)位姿作為下一次的先驗(yàn)，不斷迭代求解新的后驗(yàn)位姿，直到最近兩次位姿變化不超過第一閾值，即位姿收斂；[0018]步驟1-7,將前三幀均作為關(guān)鍵幀，當(dāng)當(dāng)前幀的位姿和上一關(guān)鍵幀的距離相差超過第二閾值時(shí)，將當(dāng)前幀設(shè)置為關(guān)鍵幀，作為優(yōu)化節(jié)點(diǎn)加入圖優(yōu)化中，圖優(yōu)化將連續(xù)N,(一般取值為10)幀關(guān)鍵幀的姿態(tài)作為優(yōu)化變量，關(guān)鍵幀中觀測到的像素特征點(diǎn)作為圖優(yōu)化的邊，特征點(diǎn)的投影誤差作為圖的代價(jià)，通過增大或減小圖中節(jié)點(diǎn)的姿態(tài)，使得圖的代價(jià)最小，圖優(yōu)化是現(xiàn)有技術(shù)；[0019]步驟1-8,當(dāng)滿足以下條件：當(dāng)前累積關(guān)鍵幀數(shù)目超過X?(一般取值為50)幀或累積GPS數(shù)據(jù)超過X?(一般取值為100)幀，或回環(huán)檢測數(shù)達(dá)到X?(一般取值為10)次，進(jìn)行一次圖[0020]步驟1-9,對(duì)于關(guān)鍵幀，將關(guān)鍵幀點(diǎn)云轉(zhuǎn)換到地圖坐標(biāo)系下，將第一幀點(diǎn)云的位姿作為地圖原點(diǎn)，并對(duì)地圖進(jìn)行濾波，得到點(diǎn)云地圖。(點(diǎn)云是一簇三[0027]其中R為k時(shí)刻慣性測量單元IMU的先驗(yàn)旋轉(zhuǎn)量，R是k-1時(shí)刻的后驗(yàn)旋轉(zhuǎn)量，0時(shí)刻的后驗(yàn)旋轉(zhuǎn)量為單位矩陣，φ是慣性測量單元IMU的旋轉(zhuǎn)向量，上標(biāo)人符號(hào)表示對(duì)向量取反對(duì)稱矩陣，V為k時(shí)刻的先驗(yàn)速度，為k-1時(shí)刻的后驗(yàn)速度，0時(shí)刻的后驗(yàn)速度為0,a為k時(shí)刻慣性測量單元IMU測量的加速度，ba為k時(shí)刻慣性測量單元IMU的偏差，g為重力向量，89n=[n8p.nsp,n8p.n?n?nson8o[0044]其中[R.y?表示取y軸和z軸方向的旋轉(zhuǎn)，[v]π表示取y軸和z軸方向的速度，I?為向位置噪聲、y方向速度噪聲、z方向速度噪聲、x方向角度噪聲、y方向角度噪聲和z方[0049]Pk=Pk-δpk[0054]其中P&為后驗(yàn)位置，為后驗(yàn)速度，R為后驗(yàn)旋轉(zhuǎn)量，b為后驗(yàn)慣性測量單元IMU加速度計(jì)偏差，b為后驗(yàn)慣性測量單元IMU陀螺儀偏差，Pk為先驗(yàn)位置，V為先驗(yàn)速度，R.為先驗(yàn)旋轉(zhuǎn)量，b為先驗(yàn)慣性測量單元IMU加速度計(jì)偏差，b為螺儀偏差，δP為后驗(yàn)位置誤差，80為后驗(yàn)速度誤差，88為后驗(yàn)角度誤差，86為后驗(yàn)加速度計(jì)偏差的誤差，8b為后驗(yàn)陀螺儀偏差的誤差。[0055]所述視覺慣性里程計(jì)構(gòu)建模塊具體執(zhí)行如下步驟：[0056]步驟2-1,預(yù)處理階段：在視覺相機(jī)每一幀中提取Harris(以Harris命名的視覺特征點(diǎn)檢測方法)角點(diǎn)，作為視覺特征點(diǎn)，使用Lukas-Kanade光流(以Lukas和Kanade命名的視覺特征點(diǎn)追蹤方法)追蹤相鄰幀的特征點(diǎn)，并使用RANSAC(隨機(jī)采樣一致性)去除異常追蹤點(diǎn)，同時(shí)對(duì)慣性測量單元IMU數(shù)據(jù)進(jìn)行預(yù)積分，得到當(dāng)前時(shí)刻[0057]步驟2-2,初始化階段：僅采用視覺恢復(fù)前面一定幀(一般為5～10幀)的位姿和特[0058]步驟2-3,后端優(yōu)化階段：將慣性測量單元IMU的誤差和視覺誤差一起進(jìn)行非線性優(yōu)化。其中IMU的誤差為實(shí)際測量值和待優(yōu)化量(位置、速度、旋轉(zhuǎn)和IMU參數(shù))之間的偏差，視覺誤差為特征點(diǎn)位置投影后的誤差。優(yōu)化每一幀的位置、速度、旋轉(zhuǎn)和慣性測量單元IMU參數(shù)、特征點(diǎn)位置，輸出當(dāng)前位姿的初始值，輸出連續(xù)的位姿和視覺特征作為視覺慣性里程[0059]所述視覺匹配定位模塊具體執(zhí)行如下步驟：[0060]步驟3-1,對(duì)點(diǎn)云地圖進(jìn)行預(yù)處理，在匹配前對(duì)點(diǎn)云地圖進(jìn)行廣度優(yōu)先搜索BFS聚類，篩去點(diǎn)數(shù)少于X?個(gè)的點(diǎn)云簇，將整張點(diǎn)云地圖分割為長方體的網(wǎng)格，長方體網(wǎng)格的尺寸為40m*40m*40m,并以當(dāng)前位置所在網(wǎng)格為中心構(gòu)建子圖，子圖為5*5*3的網(wǎng)格；[0061]步驟3-2,對(duì)于當(dāng)前相機(jī)視覺幀的特征點(diǎn)，在視覺慣性里程計(jì)中已經(jīng)對(duì)其深度進(jìn)行估計(jì)，將特征點(diǎn)根據(jù)當(dāng)前幀位姿投影到地圖坐標(biāo)系下，使用kd-tree算法在當(dāng)前子圖中搜索最近的地圖點(diǎn)作為匹配對(duì)應(yīng)點(diǎn)；[0062]步驟3-3,設(shè)定步驟3-2中視覺特征點(diǎn)在地圖中的對(duì)應(yīng)點(diǎn)對(duì)數(shù)為N,將N對(duì)點(diǎn)隨機(jī)排[0063]步驟3-4,使用對(duì)偶四元數(shù)表示當(dāng)前幀的位姿，其中q為旋轉(zhuǎn)四元[0064]設(shè)定步驟3-2中正確的匹配點(diǎn)對(duì)比例為X?,步驟3-3中8對(duì)點(diǎn)的集合作為局內(nèi)點(diǎn)，采用RANSAC算法估計(jì)出當(dāng)前的位姿；[0065]步驟3-5,使用當(dāng)前的位姿將視覺特征點(diǎn)投影到地圖坐標(biāo)系，并查詢最近點(diǎn)對(duì)，重復(fù)步驟3-3～步驟3-4,直到最新估計(jì)的位姿旋轉(zhuǎn)角度變化不超過第三閾值，且平移不超過第四閾值，或者迭代次數(shù)超過第五閾值，輸出最終的位姿作為定位結(jié)果。[0066]有益效果：1.本發(fā)明提供了一種高精度的點(diǎn)云地圖建立方法，為機(jī)器人定位、三維重建提供了基礎(chǔ)。2.本發(fā)明提出了一種基于點(diǎn)云地圖的視覺慣性定位方法，該方法解決了視覺定位和激光定位方案存在的位姿漂移問題。3.本發(fā)明提供了一種基于低成本的定位方案，該方案僅基于點(diǎn)云地圖以及視覺和IMU傳感器，該地圖可以復(fù)用。4.本發(fā)明提出了一種將視覺和點(diǎn)云地圖進(jìn)行匹配的方法，提高了定位的準(zhǔn)確率。附圖說明[0067]下面結(jié)合附圖和具體實(shí)施方式對(duì)本發(fā)明做更進(jìn)一步的具體說明，本發(fā)明的上述和/或其他方面的優(yōu)點(diǎn)將會(huì)變得更加清楚。[0068]圖1是基于點(diǎn)云地圖的視覺慣性定位系統(tǒng)整體流程示意圖。[0069]圖2是多傳感器融合建立點(diǎn)云地圖流程示意圖。[0070]圖3是單目視覺慣性里程計(jì)流程示意圖。[0071]圖4是基于已知點(diǎn)云地圖的定位流程示意圖。[0072]圖5是基于點(diǎn)云地圖的視覺慣性定位實(shí)驗(yàn)結(jié)果示意圖。具體實(shí)施方式[0073]圖1為基于點(diǎn)云地圖的視覺慣性定位系統(tǒng)整體流程，圖4是基于已知點(diǎn)云地圖的定位流程示意圖。本發(fā)明所述方法包括點(diǎn)云地圖生成模塊、視覺慣性里程計(jì)構(gòu)建模塊、以及基于已有點(diǎn)云地圖的視覺匹配定位模塊。[0074]圖2為點(diǎn)云地圖生成模塊的功能示意圖，將IMU、激光和GPS進(jìn)行融合，實(shí)現(xiàn)多傳感相對(duì)于激光進(jìn)行對(duì)齊。對(duì)于每一幀激光點(diǎn)云，使用高頻率的IMU進(jìn)行去畸變處理。[0076]步驟1-2,對(duì)IMU進(jìn)行慣性解算，解算結(jié)果作為卡爾曼濾波的預(yù)測更新。慣性解算，步驟如下：[0082]上式計(jì)算從k-1時(shí)刻到k時(shí)刻旋轉(zhuǎn)、速度以及位移的先驗(yàn)值。[0083]